diff --git a/etc/cmake/FindSOEM.cmake b/etc/cmake/FindSOEM.cmake
deleted file mode 100644
index 529827d54e4ca1e35e69b67926f75dfe6b70113d..0000000000000000000000000000000000000000
--- a/etc/cmake/FindSOEM.cmake
+++ /dev/null
@@ -1,63 +0,0 @@
-# - Try to find soem
-# Once done this will define
-#
-#  SOEM_FOUND - soem found
-#  SOEM_INCLUDE_DIR - the soem include directory
-#  SOEM_LIBRARIES - soem library
-#
-
-if(NOT "$ENV{SOEM_DIR}" EQUAL "")
-    set(SOEM_DIR $ENV{SOEM_DIR} CACHE PATH "Path to SOEM" FORCE)
-endif()
-
-set(HEADER_SEARCH_PATHS
-    ${SOEM_DIR}/include/
-    ${SOEM_DIR}/include/soem/
-    ${SOEM_DIR}/../soem/         #if SOEM_DIR points to build
-    ${SOEM_DIR}/../osal/         #if SOEM_DIR points to build
-    ${SOEM_DIR}/../oshw/         #if SOEM_DIR points to build
-    ${SOEM_DIR}/../osal/linux/   #if SOEM_DIR points to build
-    ${SOEM_DIR}/../oshw/linux/   #if SOEM_DIR points to build
-    ENV CPATH
-    /usr/include/
-    /usr/include/soem/
-    /usr/include/
-    /usr/local/include/soem/
-    /opt/local/include/soem/
-)
-
-#if soem is used from build, all headers are scattered around
-#find all of them and add them to SOEM_INCLUDE_DIR
-find_path(SOEM_INCLUDE_DIR_0 NAMES ethercatmain.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-
-find_path(SOEM_INCLUDE_DIR_1 NAMES osal.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-find_path(SOEM_INCLUDE_DIR_2 NAMES osal_defs.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-
-find_path(SOEM_INCLUDE_DIR_3 NAMES oshw.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-find_path(SOEM_INCLUDE_DIR_4 NAMES nicdrv.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-
-if(SOEM_INCLUDE_DIR_0 AND SOEM_INCLUDE_DIR_1 AND SOEM_INCLUDE_DIR_2 AND SOEM_INCLUDE_DIR_3 AND SOEM_INCLUDE_DIR_4)
-    set(SOEM_INCLUDE_DIR ${SOEM_INCLUDE_DIR_0} ${SOEM_INCLUDE_DIR_1} ${SOEM_INCLUDE_DIR_2} ${SOEM_INCLUDE_DIR_3} ${SOEM_INCLUDE_DIR_4})
-endif()
-
-FIND_LIBRARY(SOEM_LIBRARIES NAMES soem
-  PATHS
-  ${SOEM_DIR}                #if SOEM_DIR points to build
-  ${SOEM_DIR}/lib
-  ENV LD_LIBRARY_PATH
-  ENV LIBRARY_PATH
-  /usr/lib
-  /usr/local/lib
-  /opt/local/lib
-  NO_DEFAULT_PATH
-)
-
-include(FindPackageHandleStandardArgs)
-# handle the QUIETLY and REQUIRED arguments and set OODL_YOUBOT_FOUND to TRUE
-# if all listed variables are TRUE
-find_package_handle_standard_args(SOEM  DEFAULT_MSG
-                                  SOEM_LIBRARIES SOEM_INCLUDE_DIR)
-
-
-# show the SOEM_INCLUDE_DIR and SOEM_LIBRARY_DIR variables only in the advanced view
-MARK_AS_ADVANCED(SOEM_INCLUDE_DIR SOEM_LIBRARIES)
diff --git a/scenarios/ArMemObjectMemory/config/global.cfg b/scenarios/ArMemObjectMemory/config/global.cfg
index 39e1d4a1aac92cf8ffdb032a033f2d88d9534601..e2c9fcd96df8476e881682f9915ed92f0b269ac9 100644
--- a/scenarios/ArMemObjectMemory/config/global.cfg
+++ b/scenarios/ArMemObjectMemory/config/global.cfg
@@ -39,7 +39,7 @@ RobotConfig.PlatformName = Platform
 #  - Default:            ::NOT_DEFINED::
 #  - Case sensitivity:   no
 #  - Required:           no
-RobotConfig.RobotFileName = Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml
+RobotConfig.RobotFileName = armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml
 
 
 # RobotConfig.RobotNodeSetName:  Custom Property
diff --git a/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
index c32f3a428572868209dc0789f6fbee00f37c1327..907e852da2eb889fd42e47bb09c214e9be96b091 100644
--- a/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
+++ b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
@@ -1,7 +1,7 @@
 
 armarx_component_set_name(RobotControlUI)
 
-set(COMPONENT_LIBS RobotAPIOperations Simox::SimoxUtility)
+set(COMPONENT_LIBS Simox::SimoxUtility ArmarXCore RobotAPIOperations)
 
 set(SOURCES main.cpp
     RobotControlUI.cpp
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 87966a2f7502ab85b3984fbff927b3ed9e8a03f1..c05ba4c73fed76887733249df491d266935035ec 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -155,7 +155,7 @@ namespace armarx
             pos.y() = i * 200.0f;
             viz::Robot robot = viz::Robot(name)
                                .position(pos)
-                               .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml")
+                               .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml")
                                .overrideColor(simox::Color::green(64 + i * 8));
 
             layer.add(robot);
@@ -266,7 +266,7 @@ namespace armarx
 
             viz::Robot robot = viz::Robot("robot")
                                .position(pos)
-                               .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml");
+                               .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
 
             // Full model
             if (true)
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/main.cpp b/source/RobotAPI/components/DSObstacleAvoidance/main.cpp
index 3443a1532116011cd679198f9727563b4ff3885d..350f59edd246a0f74e3cbabfb3f95871f4d12cf0 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/main.cpp
+++ b/source/RobotAPI/components/DSObstacleAvoidance/main.cpp
@@ -29,7 +29,7 @@
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
-// Armar6Skills
+// armar6_skills
 #include <RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h>
 
 
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index f15496da384cd8c25169bdcc5decf93b730cff6e..6ca25b01ef9eb704a2f29140e54e6e602c8b32b6 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -13,7 +13,7 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * @package    Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package    armar6_skills::ArmarXObjects::DynamicObstacleManager
  * @author     Fabian PK ( fabian dot peller-konrad at kit dot edu )
  * @date       2020
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index f0820c595d0651933255fc1d207f54c40f6c8aae..cdabab3966b27c232b14c6e4f31a2070eba12e8f 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -13,7 +13,7 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * @package    Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package    armar6_skills::ArmarXObjects::DynamicObstacleManager
  * @author     Fabian PK ( fabian dot peller-konrad at kit dot edu )
  * @date       2020
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index 3cebd4612e8c94ddb015480e8079dd5859ed741e..91af23ed518e9348e3d37cd71ddcdcc2b8646ff8 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -13,7 +13,7 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * @package    Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package    armar6_skills::ArmarXObjects::DynamicObstacleManager
  * @author     Fabian PK ( fabian dot peller-konrad at kit dot edu )
  * @date       2020
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
index 95b7491d2d08e989321dcfdf30463acd46a95e9d..5dd70f211733cc61469a0d8b2be38f242b3307ff 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
@@ -13,7 +13,7 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * @package    Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package    armar6_skills::ArmarXObjects::DynamicObstacleManager
  * @author     Fabian PK ( fabian dot peller-konrad at kit dot edu )
  * @date       2020
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
diff --git a/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp b/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
index 52fa2345cd6c12f0cd6fd7cecb3a43564731605a..3184203cd117e51e8e9748ec823208089e40a488 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
@@ -13,19 +13,19 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * @package    Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package    armar6_skills::ArmarXObjects::DynamicObstacleManager
  * @author     Fabian PK ( fabian dot peller-konrad at kit dot edu )
  * @date       2020
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
 
-#define BOOST_TEST_MODULE Armar6Skills::ArmarXObjects::DynamicObstacleManager
+#define BOOST_TEST_MODULE armar6_skills::ArmarXObjects::DynamicObstacleManager
 
 #define ARMARX_BOOST_TEST
 
 #include <RobotAPI/Test.h>
-#include <Armar6Skills/components/DynamicObstacleManager/DynamicObstacleManager.h>
+#include <armar6_skills/components/DynamicObstacleManager/DynamicObstacleManager.h>
 
 #include <iostream>
 
diff --git a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
index 23861609c8852bae3dcfcc013e03ecb13d41c704..b9bd6eec7a825086be99f55c005948c73b159c2b 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
+++ b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
@@ -32,4 +32,3 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
 if(Eigen3_FOUND)
     target_include_directories(EarlyVisionGraph SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR} ${Eigen3_INCLUDE_DIRS})
 endif()
-
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index 65fa2f6ddf1046e8d578798cbb94023110a4907f..8eb4b47b27281850862e414d80654c61639e3ba7 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -300,8 +300,8 @@ namespace armarx
 
     void NaturalIKTest::testTaskRun()
     {
-        CMakePackageFinder finder("Armar6RT");
-        std::string robotFile = finder.getDataDir() + "/Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml";
+        CMakePackageFinder finder("armar6_rt");
+        std::string robotFile = finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
         ARMARX_IMPORTANT << "loading robot from " << robotFile;
         VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
 
@@ -584,12 +584,12 @@ namespace armarx
         viz::Layer layer_robot = arviz.layer("Robot");
         viz::Robot vizrobot = viz::Robot("robot")
                               .position(Eigen::Vector3f::Zero())
-                              .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml");
+                              .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
         vizrobot.useFullModel();
         layer_robot.add(vizrobot);
 
-        CMakePackageFinder finder("Armar6RT");
-        std::string robotFile = finder.getDataDir() + "/Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml";
+        CMakePackageFinder finder("armar6_rt");
+        std::string robotFile = finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
         ARMARX_IMPORTANT << "loading robot from " << robotFile;
         VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
 
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 94b1deab94ab62fa3707552fea3ef92560ecba67..374fae4c45ac36a164b11136d08f6e43384e9933 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -538,23 +538,24 @@ namespace armarx
         const IceUtil::Time newestTimeInHistory = jointHistory.rbegin()->first;
         if (time > newestTimeInHistory)
         {
-            IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
-            if (time <= newestTimeInHistory + maxOffset)
-            {
-                ARMARX_INFO << deactivateSpam(5)
-                            << "Requested joint timestamp is newer than newest available timestamp!"
-                            << "\n- requested timestamp: \t" << time.toDateTime()
-                            << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime()
-                            << "\n- difference:          \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
-            }
-            else
+            const IceUtil::Time minOffset = IceUtil::Time::milliSeconds(25);
+            const IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
+            if (time > newestTimeInHistory + maxOffset)
             {
-                ARMARX_WARNING << deactivateSpam(1) << "Requested joint timestamp is substantially newer (>"
+                ARMARX_WARNING << deactivateSpam(5) << "Requested joint timestamp is substantially newer (>"
                                << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
                                << "\n- requested timestamp: \t" << time.toDateTime()
                                << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime();
                 return std::nullopt;
             }
+            else if (time > newestTimeInHistory + minOffset)
+            {
+                ARMARX_INFO << deactivateSpam(10)
+                            << "Requested joint timestamp is newer than newest available timestamp!"
+                            << "\n- requested timestamp: \t" << time.toDateTime()
+                            << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime()
+                            << "\n- difference:          \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+            }
 
             return Timestamped<NameValueMap> {jointHistory.rbegin()->first, jointHistory.rbegin()->second};
         }
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 27aaa07ddf1c0f0dc819a9a74366036cdadfe11c..c9eae54f7d33aeeab1afede2612eea3f0c2bf868 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -66,9 +66,6 @@ namespace armarx::armem::server::robot_state
 
         defs->optional(robotUnit.reader.properties.sensorPrefix, robotUnitPrefix + "SensorValuePrefix",
                        "Prefix of all sensor values.");
-        defs->optional(robotUnit.writer.properties.memoryBatchSize, robotUnitPrefix + "MemoryBatchSize",
-                       "The size of the entity snapshot to send to the memory. Minimum is 1.")
-        .setMin(1);
         defs->optional(robotUnit.pollFrequency, robotUnitPrefix + "UpdateFrequency",
                        "The frequency to store values in Hz. All other values get discarded. "
                        "Minimum is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
@@ -102,7 +99,6 @@ namespace armarx::armem::server::robot_state
         commonVisu.init();
 
         robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, ROBOT_UNIT_MAXIMUM_FREQUENCY);
-        robotUnit.writer.properties.memoryBatchSize = std::max(static_cast<unsigned int>(1), robotUnit.writer.properties.memoryBatchSize);
 
         std::vector<std::string> includePaths;
         std::vector<std::string> packages = armarx::Application::GetProjectDependencies();
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
index 16a61f9af703665aa7d489ee115b43cef35edc27..4a147cf86218c9c5035ca27474472bca1bf23c54 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
@@ -23,11 +23,11 @@
 #ifndef _ARMARX_LIB_RobotAPI_NJointController_HPP
 #define _ARMARX_LIB_RobotAPI_NJointController_HPP
 
-#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
 #include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
 
-#include "NJointController.h"
 #include "../RobotUnit.h"
+#include "NJointController.h"
 
 namespace armarx::RobotUnitModule
 {
@@ -39,46 +39,55 @@ namespace armarx
     {
     private:
         friend class RobotUnitModule::ControllerManagement;
-        virtual NJointControllerBasePtr create(
-            RobotUnitModule::ControllerManagement* cmngr,
-            const NJointControllerConfigPtr&,
-            const VirtualRobot::RobotPtr&,
-            bool deletable,
-            bool internal,
-            const std::string& instanceName) const = 0;
-        virtual WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr&, const std::map<std::string, ConstControlDevicePtr>&, const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
-        virtual NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
+        virtual NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
+                                               const NJointControllerConfigPtr&,
+                                               const VirtualRobot::RobotPtr&,
+                                               bool deletable,
+                                               bool internal,
+                                               const std::string& instanceName) const = 0;
+        virtual WidgetDescription::WidgetPtr
+        GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+                                  const std::map<std::string, ConstControlDevicePtr>&,
+                                  const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
+        virtual NJointControllerConfigPtr
+        GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
         virtual bool hasRemoteConfiguration() const = 0;
+
     protected:
         static thread_local bool ConstructorIsRunning_;
+
     public:
-        static bool ConstructorIsRunning()
+        virtual ~NJointControllerRegistryEntry() = default;
+        static bool
+        ConstructorIsRunning()
         {
             return ConstructorIsRunning_;
         }
     };
     using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
 
-    template<class ControllerType>
+    template <class ControllerType>
     struct NJointControllerRegistration;
 
     template <typename ControlDataStruct>
-    class NJointControllerWithTripleBuffer: public SynchronousNJointController
+    class NJointControllerWithTripleBuffer : public SynchronousNJointController
     {
     public:
         using MutexType = std::recursive_mutex;
         using LockGuardType = std::lock_guard<std::recursive_mutex>;
 
-        NJointControllerWithTripleBuffer(const ControlDataStruct& initialCommands = ControlDataStruct()):
-            controlDataTripleBuffer {initialCommands}
+        NJointControllerWithTripleBuffer(
+            const ControlDataStruct& initialCommands = ControlDataStruct()) :
+            controlDataTripleBuffer{initialCommands}
         {
         }
 
-        void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+        void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+                                const IceUtil::Time& timeSinceLastIteration) override;
 
     protected:
         const ControlDataStruct& rtGetControlStruct() const;
-        bool                     rtUpdateControlStruct();
+        bool rtUpdateControlStruct();
 
         void writeControlStruct();
         ControlDataStruct& getWriterControlStruct();
@@ -88,138 +97,170 @@ namespace armarx
         void reinitTripleBuffer(const ControlDataStruct& initial);
 
         mutable MutexType controlDataMutex;
+
     private:
         WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
     };
 
     template <typename ControlDataStruct>
-    using NJointControllerWithTripleBufferPtr = IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
-}
+    using NJointControllerWithTripleBufferPtr =
+        IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
+} // namespace armarx
 
 //inline functions
 namespace armarx
 {
-    template<class T>
-    inline T* NJointControllerBase::useControlTarget(const std::string& deviceName, const std::string& controlMode)
+    template <class T>
+    inline T*
+    NJointControllerBase::useControlTarget(const std::string& deviceName,
+                                           const std::string& controlMode)
     {
-        static_assert(std::is_base_of<ControlTargetBase, T>::value, "The given type does not derive ControlTargetBase");
+        static_assert(std::is_base_of<ControlTargetBase, T>::value,
+                      "The given type does not derive ControlTargetBase");
         ControlTargetBase* const ptr = useControlTarget(deviceName, controlMode);
         return ptr ? ptr->asA<T>() : nullptr;
     }
-    template<class T>
-    inline const T* NJointControllerBase::useSensorValue(const std::string& deviceName) const
+    template <class T>
+    inline const T*
+    NJointControllerBase::useSensorValue(const std::string& deviceName) const
     {
-        static_assert(std::is_base_of<SensorValueBase, T>::value, "The given type does not derive SensorValueBase");
+        static_assert(std::is_base_of<SensorValueBase, T>::value,
+                      "The given type does not derive SensorValueBase");
         const SensorValueBase* const ptr = useSensorValue(deviceName);
         return ptr ? ptr->asA<T>() : nullptr;
     }
 
-    inline void SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+    inline void
+    SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+                                                    const IceUtil::Time& timeSinceLastIteration)
     {
         rtRun(sensorValuesTimestamp, timeSinceLastIteration);
     }
 
-    inline bool NJointControllerBase::rtUsesControlDevice(std::size_t deviceIndex) const
+    inline bool
+    NJointControllerBase::rtUsesControlDevice(std::size_t deviceIndex) const
     {
         return controlDeviceUsedBitmap.at(deviceIndex);
     }
 
-    inline StringStringDictionary NJointControllerBase::getControlDeviceUsedControlModeMap(const Ice::Current&) const
+    inline StringStringDictionary
+    NJointControllerBase::getControlDeviceUsedControlModeMap(const Ice::Current&) const
     {
         return controlDeviceControlModeMap;
     }
 
-    inline const std::vector<char>& NJointControllerBase::getControlDeviceUsedBitmap() const
+    inline const std::vector<char>&
+    NJointControllerBase::getControlDeviceUsedBitmap() const
     {
         return controlDeviceUsedBitmap;
     }
 
-    inline const std::vector<std::size_t>& NJointControllerBase::rtGetControlDeviceUsedIndices() const
+    inline const std::vector<std::size_t>&
+    NJointControllerBase::rtGetControlDeviceUsedIndices() const
     {
         return controlDeviceUsedIndices;
     }
 
-    inline const std::vector<std::size_t>& NJointControllerBase::getControlDeviceUsedIndices() const
+    inline const std::vector<std::size_t>&
+    NJointControllerBase::getControlDeviceUsedIndices() const
     {
         return controlDeviceUsedIndices;
     }
 
-    inline const std::map<std::string, const JointController*>& NJointControllerBase::getControlDevicesUsedJointController()
+    inline const std::map<std::string, const JointController*>&
+    NJointControllerBase::getControlDevicesUsedJointController()
     {
         return controlDeviceUsedJointController;
     }
 
-    inline std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
+    inline std::optional<std::vector<char>>
+    NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
     {
         return isNotInConflictWith(other->getControlDeviceUsedBitmap());
     }
 
-    inline std::string NJointControllerBase::getDefaultName() const
+    inline std::string
+    NJointControllerBase::getDefaultName() const
     {
         return getClassName();
     }
 
-    inline bool NJointControllerBase::isControllerActive(const Ice::Current&) const
+    inline bool
+    NJointControllerBase::isControllerActive(const Ice::Current&) const
     {
         return isActive;
     }
 
-    inline bool NJointControllerBase::isControllerRequested(const Ice::Current&) const
+    inline bool
+    NJointControllerBase::isControllerRequested(const Ice::Current&) const
     {
         return isRequested;
     }
 
-    inline bool NJointControllerBase::isDeletable(const Ice::Current&) const
+    inline bool
+    NJointControllerBase::isDeletable(const Ice::Current&) const
     {
         return deletable;
     }
 
-    inline bool NJointControllerBase::hasControllerError(const Ice::Current&) const
+    inline bool
+    NJointControllerBase::hasControllerError(const Ice::Current&) const
     {
         return deactivatedBecauseOfError;
     }
 
-    inline std::string NJointControllerBase::getInstanceName(const Ice::Current&) const
+    inline std::string
+    NJointControllerBase::getInstanceName(const Ice::Current&) const
     {
         return instanceName_;
     }
 
-    inline WidgetDescription::StringWidgetDictionary NJointControllerBase::getFunctionDescriptions(const Ice::Current&) const
+    inline WidgetDescription::StringWidgetDictionary
+    NJointControllerBase::getFunctionDescriptions(const Ice::Current&) const
     {
         return {};
     }
 
-    inline void NJointControllerBase::callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current&)
+    inline void
+    NJointControllerBase::callDescribedFunction(const std::string&,
+                                                const StringVariantBaseMap&,
+                                                const Ice::Current&)
     {
     }
 
-    inline void NJointControllerBase::rtSetErrorState()
+    inline void
+    NJointControllerBase::rtSetErrorState()
     {
         errorState.store(true);
     }
 
-    inline bool NJointControllerBase::rtGetErrorState() const
+    inline bool
+    NJointControllerBase::rtGetErrorState() const
     {
         return errorState;
     }
 
-    inline std::size_t NJointControllerBase::rtGetNumberOfUsedControlDevices() const
+    inline std::size_t
+    NJointControllerBase::rtGetNumberOfUsedControlDevices() const
     {
         return controlDeviceUsedIndices.size();
     }
 
-    inline const std::string& NJointControllerBase::rtGetClassName() const
+    inline const std::string&
+    NJointControllerBase::rtGetClassName() const
     {
         return rtClassName_;
     }
-    inline const std::string& NJointControllerBase::rtGetInstanceName() const
+    inline const std::string&
+    NJointControllerBase::rtGetInstanceName() const
     {
         return instanceName_;
     }
 
     //NJointControllerWithTripleBuffer<ControlDataStruct>
     template <typename ControlDataStruct>
-    inline void NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun(
+    inline void
+    NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun(
         const IceUtil::Time& sensorValuesTimestamp,
         const IceUtil::Time& timeSinceLastIteration)
     {
@@ -228,19 +269,22 @@ namespace armarx
     }
 
     template <typename ControlDataStruct>
-    inline const ControlDataStruct& NJointControllerWithTripleBuffer<ControlDataStruct>::rtGetControlStruct() const
+    inline const ControlDataStruct&
+    NJointControllerWithTripleBuffer<ControlDataStruct>::rtGetControlStruct() const
     {
         return controlDataTripleBuffer.getReadBuffer();
     }
 
     template <typename ControlDataStruct>
-    inline bool NJointControllerWithTripleBuffer<ControlDataStruct>::rtUpdateControlStruct()
+    inline bool
+    NJointControllerWithTripleBuffer<ControlDataStruct>::rtUpdateControlStruct()
     {
         return controlDataTripleBuffer.updateReadBuffer();
     }
 
     template <typename ControlDataStruct>
-    inline void NJointControllerWithTripleBuffer<ControlDataStruct>::writeControlStruct()
+    inline void
+    NJointControllerWithTripleBuffer<ControlDataStruct>::writeControlStruct()
     {
         //just lock to be save and reduce the impact of an error
         //also this allows code to unlock the mutex before calling this function
@@ -250,34 +294,40 @@ namespace armarx
     }
 
     template <typename ControlDataStruct>
-    inline ControlDataStruct& NJointControllerWithTripleBuffer<ControlDataStruct>::getWriterControlStruct()
+    inline ControlDataStruct&
+    NJointControllerWithTripleBuffer<ControlDataStruct>::getWriterControlStruct()
     {
         return controlDataTripleBuffer.getWriteBuffer();
     }
 
     template <typename ControlDataStruct>
-    inline void NJointControllerWithTripleBuffer<ControlDataStruct>::setControlStruct(const ControlDataStruct& newStruct)
+    inline void
+    NJointControllerWithTripleBuffer<ControlDataStruct>::setControlStruct(
+        const ControlDataStruct& newStruct)
     {
-        LockGuardType lock {controlDataMutex};
+        LockGuardType lock{controlDataMutex};
         getWriterControlStruct() = newStruct;
         writeControlStruct();
     }
 
     template <typename ControlDataStruct>
-    inline void NJointControllerWithTripleBuffer<ControlDataStruct>::reinitTripleBuffer(const ControlDataStruct& initial)
+    inline void
+    NJointControllerWithTripleBuffer<ControlDataStruct>::reinitTripleBuffer(
+        const ControlDataStruct& initial)
     {
         controlDataTripleBuffer.reinitAllBuffers(initial);
     }
-}
+} // namespace armarx
 
 namespace armarx
 {
-    template<class ItT>
-    inline std::optional<std::vector<char>> NJointControllerBase::AreNotInConflict(ItT first, ItT last)
+    template <class ItT>
+    inline std::optional<std::vector<char>>
+    NJointControllerBase::AreNotInConflict(ItT first, ItT last)
     {
         if (first == last)
         {
-            return std::vector<char> {};
+            return std::vector<char>{};
         }
         std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
         std::vector<char> inuse(n, false);
@@ -293,43 +343,53 @@ namespace armarx
         }
         return inuse;
     }
-}
+} // namespace armarx
 namespace armarx::detail
 {
-    ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigDescription,  GenerateConfigDescription,
-                                          NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
-    ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigFromVariants,  GenerateConfigFromVariants,
-                                          NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
-
-    template<class NJointControllerT>
+    ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+        hasGenerateConfigDescription,
+        GenerateConfigDescription,
+        NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
+    ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+        hasGenerateConfigFromVariants,
+        GenerateConfigFromVariants,
+        NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
+
+    template <class NJointControllerT>
     class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
     {
-        static_assert(
-            hasGenerateConfigDescription<NJointControllerT>::value ==
-            hasGenerateConfigFromVariants<NJointControllerT>::value,
-            "Either overload both GenerateConfigDescription and GenerateConfigFromVariants, or none!"
-        );
-        static constexpr bool hasRemoteConfiguration_ = hasGenerateConfigDescription<NJointControllerT>::value;
-
-        NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
-                                       const NJointControllerConfigPtr& config,
-                                       const VirtualRobot::RobotPtr& rob,
-                                       bool deletable,
-                                       bool internal,
-                                       const std::string& instanceName) const final override
+        static_assert(hasGenerateConfigDescription<NJointControllerT>::value ==
+                          hasGenerateConfigFromVariants<NJointControllerT>::value,
+                      "Either overload both GenerateConfigDescription and "
+                      "GenerateConfigFromVariants, or none!");
+        static constexpr bool hasRemoteConfiguration_ =
+            hasGenerateConfigDescription<NJointControllerT>::value;
+
+        NJointControllerBasePtr
+        create(RobotUnitModule::ControllerManagement* cmngr,
+               const NJointControllerConfigPtr& config,
+               const VirtualRobot::RobotPtr& rob,
+               bool deletable,
+               bool internal,
+               const std::string& instanceName) const final override
         {
             ARMARX_CHECK_EXPRESSION(cmngr) << "ControllerManagement module is NULL!";
 
             using ConfigPtrT = typename NJointControllerT::ConfigPtrT;
             ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
-            ARMARX_CHECK_EXPRESSION(cfg) << "The configuration is of the wrong type! it has to be an instance of: "
-                                         << GetTypeString<ConfigPtrT>();
+            ARMARX_CHECK_EXPRESSION(cfg)
+                << "The configuration is of the wrong type! it has to be an instance of: "
+                << GetTypeString<ConfigPtrT>();
 
-            ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning()) << "Two NJointControllers are created at the same time";
+            ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning())
+                << "Two NJointControllers are created at the same time";
             NJointControllerBasePtr ptr;
             {
                 ConstructorIsRunning_ = true;
-                ARMARX_ON_SCOPE_EXIT{ConstructorIsRunning_ = false;};
+                ARMARX_ON_SCOPE_EXIT
+                {
+                    ConstructorIsRunning_ = false;
+                };
                 ptr = new NJointControllerT(cmngr->_modulePtr<RobotUnit>(), cfg, rob);
             }
             ptr->deletable = deletable;
@@ -339,35 +399,42 @@ namespace armarx::detail
             return ptr;
         }
 
-        WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr& robot,
-                const std::map<std::string, ConstControlDevicePtr>& controlDevices,
-                const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
+        WidgetDescription::WidgetPtr
+        GenerateConfigDescription(
+            const VirtualRobot::RobotPtr& robot,
+            const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+            const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
         {
-            if constexpr(hasRemoteConfiguration_)
+            if constexpr (hasRemoteConfiguration_)
             {
                 try
                 {
-                    return NJointControllerT::GenerateConfigDescription(robot, controlDevices, sensorDevices);
+                    return NJointControllerT::GenerateConfigDescription(
+                        robot, controlDevices, sensorDevices);
                 }
                 catch (Ice::UserException& e)
                 {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigDescription'"
+                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                 << "::GenerateConfigDescription'"
                                  << "\n---- file = " << e.ice_file()
                                  << "\n---- line = " << e.ice_line()
-                                 << "\n---- id   = " << e.ice_id()
-                                 << "\n---- what:\n" << e.what()
-                                 << "\n---- stacktrace:\n" << e.ice_stackTrace();
+                                 << "\n---- id   = " << e.ice_id() << "\n---- what:\n"
+                                 << e.what() << "\n---- stacktrace:\n"
+                                 << e.ice_stackTrace();
                     throw;
                 }
                 catch (std::exception& e)
                 {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigDescription'"
-                                 << "\n---- what:\n" << e.what();
+                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                 << "::GenerateConfigDescription'"
+                                 << "\n---- what:\n"
+                                 << e.what();
                     throw;
                 }
                 catch (...)
                 {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigDescription'";
+                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                 << "::GenerateConfigDescription'";
                     throw;
                 }
             }
@@ -377,9 +444,10 @@ namespace armarx::detail
             }
         }
 
-        NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
+        NJointControllerConfigPtr
+        GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
         {
-            if constexpr(hasRemoteConfiguration_)
+            if constexpr (hasRemoteConfiguration_)
             {
                 try
                 {
@@ -387,23 +455,27 @@ namespace armarx::detail
                 }
                 catch (Ice::UserException& e)
                 {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigFromVariants'"
+                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                 << "::GenerateConfigFromVariants'"
                                  << "\n---- file = " << e.ice_file()
                                  << "\n---- line = " << e.ice_line()
-                                 << "\n---- id   = " << e.ice_id()
-                                 << "\n---- what:\n" << e.what()
-                                 << "\n---- stacktrace:\n" << e.ice_stackTrace();
+                                 << "\n---- id   = " << e.ice_id() << "\n---- what:\n"
+                                 << e.what() << "\n---- stacktrace:\n"
+                                 << e.ice_stackTrace();
                     throw;
                 }
                 catch (std::exception& e)
                 {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigFromVariants'"
-                                 << "\n---- what:\n" << e.what();
+                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                 << "::GenerateConfigFromVariants'"
+                                 << "\n---- what:\n"
+                                 << e.what();
                     throw;
                 }
                 catch (...)
                 {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigFromVariants'";
+                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                 << "::GenerateConfigFromVariants'";
                     throw;
                 }
             }
@@ -413,31 +485,35 @@ namespace armarx::detail
             }
         }
 
-        bool hasRemoteConfiguration() const final override
+        bool
+        hasRemoteConfiguration() const final override
         {
             return hasRemoteConfiguration_;
         }
     };
-}
+} // namespace armarx::detail
 namespace armarx
 {
     using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
-    template<class ControllerType>
+    template <class ControllerType>
     struct NJointControllerRegistration
     {
         NJointControllerRegistration(const std::string& name)
         {
             NJointControllerRegistry::registerElement(
                 name,
-                std::unique_ptr<NJointControllerRegistryEntry>(new detail::NJointControllerRegistryEntryHelper<ControllerType>));
+                std::unique_ptr<NJointControllerRegistryEntry>(
+                    new detail::NJointControllerRegistryEntryHelper<ControllerType>));
         }
     };
-}
-#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T)                                              \
-    static_assert(::armarx::detail::hasGenerateConfigDescription<T>::value ,                                \
-                  #T " does not offer a construction gui (missing: static GenerateConfigDescription)" );    \
-    static_assert(::armarx::detail::hasGenerateConfigFromVariants<T>::value,                                \
-                  #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
+} // namespace armarx
+#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T)                                     \
+    static_assert(                                                                                 \
+        ::armarx::detail::hasGenerateConfigDescription<T>::value,                                  \
+        #T " does not offer a construction gui (missing: static GenerateConfigDescription)");      \
+    static_assert(                                                                                 \
+        ::armarx::detail::hasGenerateConfigFromVariants<T>::value,                                 \
+        #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
 
 
 #endif
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui
index 0858c31b0c96248ecf98505e47bcee7508b767f2..b81a24c97b94930c530b082f91ef1ad039227c04 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui
@@ -53,17 +53,17 @@
      </property>
      <item>
       <property name="text">
-       <string>Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml</string>
+       <string>armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml</string>
       </property>
      </item>
      <item>
       <property name="text">
-       <string>Armar6RT/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml</string>
+       <string>armar6_rt/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml</string>
       </property>
      </item>
      <item>
       <property name="text">
-       <string>Armar6RT/robotmodel/Armar6-SH/Armar6-LeftHand-v3.xml</string>
+       <string>armar6_rt/robotmodel/Armar6-SH/Armar6-LeftHand-v3.xml</string>
       </property>
      </item>
     </widget>
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp
deleted file mode 100644
index f521730131e1f3e3a70e1c037d18381cd0a1f1cc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::BimanualCartesianAdmittanceControllerGuiGuiPlugin
- * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date       2020
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "BimanualCartesianAdmittanceControllerGuiGuiPlugin.h"
-
-#include "BimanualCartesianAdmittanceControllerGuiWidgetController.h"
-
-namespace armarx
-{
-    BimanualCartesianAdmittanceControllerGuiGuiPlugin::BimanualCartesianAdmittanceControllerGuiGuiPlugin()
-    {
-        addWidget < BimanualCartesianAdmittanceControllerGuiWidgetController > ();
-    }
-}
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.h b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.h
deleted file mode 100644
index 095a535247b070c039a680cecb3031c951c750ca..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::BimanualCartesianAdmittanceControllerGui
- * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date       2020
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-
-namespace armarx
-{
-    /**
-     * \class BimanualCartesianAdmittanceControllerGuiGuiPlugin
-     * \ingroup ArmarXGuiPlugins
-     * \brief BimanualCartesianAdmittanceControllerGuiGuiPlugin brief description
-     *
-     * Detailed description
-     */
-    class ARMARXCOMPONENT_IMPORT_EXPORT BimanualCartesianAdmittanceControllerGuiGuiPlugin:
-        public armarx::ArmarXGuiPlugin
-    {
-        Q_OBJECT
-        Q_INTERFACES(ArmarXGuiInterface)
-        Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
-    public:
-        /**
-         * All widgets exposed by this plugin are added in the constructor
-         * via calls to addWidget()
-         */
-        BimanualCartesianAdmittanceControllerGuiGuiPlugin();
-    };
-}
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidget.ui b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidget.ui
deleted file mode 100644
index 0b5b4e258a5db005fb5e2a666baa3f4073085a8c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidget.ui
+++ /dev/null
@@ -1,2344 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>BimanualCartesianAdmittanceControllerGuiWidget</class>
- <widget class="QWidget" name="BimanualCartesianAdmittanceControllerGuiWidget">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>816</width>
-    <height>1382</height>
-   </rect>
-  </property>
-  <property name="windowTitle">
-   <string>BimanualCartesianAdmittanceControllerGuiWidget</string>
-  </property>
-  <layout class="QGridLayout" name="gridLayout">
-   <item row="0" column="0" colspan="4">
-    <widget class="QScrollArea" name="scrollArea">
-     <property name="widgetResizable">
-      <bool>true</bool>
-     </property>
-     <widget class="QWidget" name="scrollAreaWidgetContents">
-      <property name="geometry">
-       <rect>
-        <x>0</x>
-        <y>-31</y>
-        <width>782</width>
-        <height>1393</height>
-       </rect>
-      </property>
-      <layout class="QGridLayout" name="gridLayout_14">
-       <item row="0" column="3">
-        <widget class="QPushButton" name="pushButtonCtrlDelete">
-         <property name="text">
-          <string>Delete</string>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="0">
-        <widget class="QPushButton" name="pushButtonCtrlCreate">
-         <property name="text">
-          <string>Create</string>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="2">
-        <widget class="QPushButton" name="pushButtonCtrlDeactivate">
-         <property name="text">
-          <string>Deactivate</string>
-         </property>
-        </widget>
-       </item>
-       <item row="1" column="0" colspan="4">
-        <widget class="QGroupBox" name="groupBox">
-         <property name="title">
-          <string>Settings</string>
-         </property>
-         <property name="checkable">
-          <bool>true</bool>
-         </property>
-         <layout class="QHBoxLayout" name="horizontalLayout">
-          <property name="spacing">
-           <number>0</number>
-          </property>
-          <property name="leftMargin">
-           <number>6</number>
-          </property>
-          <property name="topMargin">
-           <number>0</number>
-          </property>
-          <property name="rightMargin">
-           <number>0</number>
-          </property>
-          <property name="bottomMargin">
-           <number>0</number>
-          </property>
-          <item>
-           <widget class="QWidget" name="widget" native="true">
-            <layout class="QGridLayout" name="gridLayout_3">
-             <property name="leftMargin">
-              <number>0</number>
-             </property>
-             <property name="topMargin">
-              <number>0</number>
-             </property>
-             <property name="rightMargin">
-              <number>0</number>
-             </property>
-             <property name="bottomMargin">
-              <number>0</number>
-             </property>
-             <property name="spacing">
-              <number>0</number>
-             </property>
-             <item row="3" column="0">
-              <widget class="QGroupBox" name="groupBox_5">
-               <property name="title">
-                <string>Admittance</string>
-               </property>
-               <property name="checkable">
-                <bool>true</bool>
-               </property>
-               <layout class="QGridLayout" name="gridLayout_6">
-                <property name="leftMargin">
-                 <number>0</number>
-                </property>
-                <property name="topMargin">
-                 <number>0</number>
-                </property>
-                <property name="rightMargin">
-                 <number>0</number>
-                </property>
-                <property name="bottomMargin">
-                 <number>0</number>
-                </property>
-                <property name="spacing">
-                 <number>0</number>
-                </property>
-                <item row="0" column="0">
-                 <widget class="QWidget" name="widget_4" native="true">
-                  <layout class="QGridLayout" name="gridLayout_10">
-                   <item row="1" column="5">
-                    <widget class="QLabel" name="label_15">
-                     <property name="text">
-                      <string>Pitch</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="0">
-                    <widget class="QLabel" name="label_5">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="text">
-                      <string>KP</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="0">
-                    <widget class="QLabel" name="label_7">
-                     <property name="text">
-                      <string>KM</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="2">
-                    <widget class="QLabel" name="label_14">
-                     <property name="text">
-                      <string>Y</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="4">
-                    <widget class="QLabel" name="label_16">
-                     <property name="text">
-                      <string>Roll</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="6">
-                    <widget class="QLabel" name="label_12">
-                     <property name="text">
-                      <string>Yaw</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="1">
-                    <widget class="QLabel" name="label_13">
-                     <property name="text">
-                      <string>X</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="0">
-                    <widget class="QLabel" name="label_6">
-                     <property name="text">
-                      <string>KD</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="3">
-                    <widget class="QLabel" name="label_17">
-                     <property name="text">
-                      <string>Z</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="0" colspan="7">
-                    <widget class="QPushButton" name="pushButtonCfgSendAdmittance">
-                     <property name="text">
-                      <string>Send</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAPTX">
-                     <property name="decimals">
-                      <number>2</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-10000.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>10000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>200.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxADTX">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>100.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAMTX">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>1.550000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxADTY">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>100.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAPTY">
-                     <property name="decimals">
-                      <number>2</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-10000.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>10000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>200.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAPTZ">
-                     <property name="decimals">
-                      <number>2</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-10000.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>10000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>200.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAMTY">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxADTZ">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>100.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAMTZ">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAPRX">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>1000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>200.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxADRX">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>70.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAMRX">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>1.550000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAPRY">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>1000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>200.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxADRY">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>70.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAMRY">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAPRZ">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>1000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>200.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxADRZ">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>70.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxAMRZ">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                  </layout>
-                 </widget>
-                </item>
-               </layout>
-              </widget>
-             </item>
-             <item row="4" column="0">
-              <widget class="QGroupBox" name="groupBox_6">
-               <property name="title">
-                <string>Force</string>
-               </property>
-               <property name="checkable">
-                <bool>true</bool>
-               </property>
-               <layout class="QGridLayout" name="gridLayout_7">
-                <property name="leftMargin">
-                 <number>0</number>
-                </property>
-                <property name="topMargin">
-                 <number>0</number>
-                </property>
-                <property name="rightMargin">
-                 <number>0</number>
-                </property>
-                <property name="bottomMargin">
-                 <number>0</number>
-                </property>
-                <property name="spacing">
-                 <number>0</number>
-                </property>
-                <item row="0" column="0">
-                 <widget class="QWidget" name="widget_3" native="true">
-                  <layout class="QGridLayout" name="gridLayout_13">
-                   <item row="1" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTXL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-20.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="7" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTXL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>2.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTXL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.003000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRXR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTYR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.001600000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="6" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTZL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.337300000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTXR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>20.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFYL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.559600000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTZR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="6" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTYL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.656500000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="4" colspan="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFMR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>1.113500000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="6" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTYR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.058900000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="7" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTZR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>2.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="7" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTXR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>2.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="7" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTYL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>2.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="8" column="0" colspan="7">
-                    <widget class="QPushButton" name="pushButtonCfgSendForce">
-                     <property name="text">
-                      <string>Send</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="1" colspan="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFML">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>1.223200000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="6" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTXL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.103300000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFZL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-1.710300000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTYL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTZL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.094400000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFYR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.651400000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="6" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTZR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.067900000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTZL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTYL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRXL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="0">
-                    <widget class="QLabel" name="label_22">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="text">
-                      <string>Wrench XYZ</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="7" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTZL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>2.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRZR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFZR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-1.397800000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTZR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.099300000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTXR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.004000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRZL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTYR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFXR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.274500000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRYL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>4.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="6" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTXR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.029000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFXL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>-0.733500000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRYR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>4.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="7" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTYR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>2.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="1" colspan="3">
-                    <widget class="QLabel" name="label_31">
-                     <property name="text">
-                      <string>Left</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="4" colspan="3">
-                    <widget class="QLabel" name="label_32">
-                     <property name="text">
-                      <string>Right</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="0">
-                    <widget class="QLabel" name="label_33">
-                     <property name="text">
-                      <string>Wrench RPY</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="0">
-                    <widget class="QLabel" name="label_34">
-                     <property name="text">
-                      <string>Mass</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="0">
-                    <widget class="QLabel" name="label_35">
-                     <property name="text">
-                      <string>Com</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="6" column="0">
-                    <widget class="QLabel" name="label_36">
-                     <property name="text">
-                      <string>Offset torqe</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="0">
-                    <widget class="QLabel" name="label_37">
-                     <property name="text">
-                      <string>Offset force</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="7" column="0">
-                    <widget class="QLabel" name="label_38">
-                     <property name="text">
-                      <string>Force thresh</string>
-                     </property>
-                    </widget>
-                   </item>
-                  </layout>
-                 </widget>
-                </item>
-               </layout>
-              </widget>
-             </item>
-             <item row="5" column="0">
-              <widget class="QGroupBox" name="groupBox_7">
-               <property name="title">
-                <string>Other</string>
-               </property>
-               <property name="checkable">
-                <bool>true</bool>
-               </property>
-               <layout class="QGridLayout" name="gridLayout_8">
-                <property name="leftMargin">
-                 <number>0</number>
-                </property>
-                <property name="topMargin">
-                 <number>0</number>
-                </property>
-                <property name="rightMargin">
-                 <number>0</number>
-                </property>
-                <property name="bottomMargin">
-                 <number>0</number>
-                </property>
-                <property name="spacing">
-                 <number>0</number>
-                </property>
-                <item row="0" column="0">
-                 <widget class="QWidget" name="widget_7" native="true">
-                  <layout class="QGridLayout" name="gridLayout_9">
-                   <item row="2" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxOTorqueLim">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>30.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxOFiltCoeff">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.500000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxOCalibTime">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>1.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxOBoxw">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.340000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="0">
-                    <widget class="QLabel" name="label_23">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="text">
-                      <string>Box width</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="0">
-                    <widget class="QLabel" name="label_24">
-                     <property name="text">
-                      <string>filter coeff</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="0">
-                    <widget class="QLabel" name="label_25">
-                     <property name="text">
-                      <string>torque limit</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="0">
-                    <widget class="QLabel" name="label_26">
-                     <property name="text">
-                      <string>ft calib time</string>
-                     </property>
-                    </widget>
-                   </item>
-                  </layout>
-                 </widget>
-                </item>
-               </layout>
-              </widget>
-             </item>
-             <item row="2" column="0">
-              <widget class="QGroupBox" name="groupBox_4">
-               <property name="title">
-                <string>Impedance</string>
-               </property>
-               <property name="checkable">
-                <bool>true</bool>
-               </property>
-               <layout class="QGridLayout" name="gridLayout_5">
-                <property name="leftMargin">
-                 <number>0</number>
-                </property>
-                <property name="topMargin">
-                 <number>0</number>
-                </property>
-                <property name="rightMargin">
-                 <number>0</number>
-                </property>
-                <property name="bottomMargin">
-                 <number>0</number>
-                </property>
-                <property name="spacing">
-                 <number>0</number>
-                </property>
-                <item row="0" column="0">
-                 <widget class="QWidget" name="widget_5" native="true">
-                  <layout class="QGridLayout" name="gridLayout_11">
-                   <item row="1" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTXR">
-                     <property name="decimals">
-                      <number>2</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-10000.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>10000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>800.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRXR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>30.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRXL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>30.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTYR">
-                     <property name="decimals">
-                      <number>2</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-10000.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>10000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>800.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRXR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>100.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTZR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>50.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRYL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>30.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTZL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>50.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTZL">
-                     <property name="decimals">
-                      <number>2</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-10000.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>10000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>800.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="0">
-                    <widget class="QLabel" name="label_28">
-                     <property name="text">
-                      <string>KD RPY</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="0" colspan="7">
-                    <widget class="QPushButton" name="pushButtonCfgSendImpedance">
-                     <property name="text">
-                      <string>Send</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRZR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>30.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRZL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>100.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTZR">
-                     <property name="decimals">
-                      <number>2</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-10000.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>10000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>800.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRYL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>100.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="3">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRZL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>30.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTXL">
-                     <property name="decimals">
-                      <number>2</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-10000.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>10000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>800.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRYR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>30.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRXL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>100.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTYR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>50.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="0">
-                    <widget class="QLabel" name="label_20">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="text">
-                      <string>KP XYZ</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTXL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>50.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="0">
-                    <widget class="QLabel" name="label_21">
-                     <property name="text">
-                      <string>KD XYZ</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTYL">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>50.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="5">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRYR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>100.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="6">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRZR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>100.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="4">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTXR">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>50.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="0">
-                    <widget class="QLabel" name="label_27">
-                     <property name="text">
-                      <string>KP RPY</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="2">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTYL">
-                     <property name="decimals">
-                      <number>2</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-10000.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>10000.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>800.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="1" colspan="3">
-                    <widget class="QLabel" name="label_29">
-                     <property name="text">
-                      <string>Left</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="4" colspan="3">
-                    <widget class="QLabel" name="label_30">
-                     <property name="text">
-                      <string>Right</string>
-                     </property>
-                    </widget>
-                   </item>
-                  </layout>
-                 </widget>
-                </item>
-               </layout>
-              </widget>
-             </item>
-             <item row="1" column="0">
-              <widget class="QGroupBox" name="groupBox_3">
-               <property name="title">
-                <string>Nullspace</string>
-               </property>
-               <property name="checkable">
-                <bool>true</bool>
-               </property>
-               <layout class="QGridLayout" name="gridLayout_4">
-                <property name="leftMargin">
-                 <number>0</number>
-                </property>
-                <property name="topMargin">
-                 <number>0</number>
-                </property>
-                <property name="rightMargin">
-                 <number>0</number>
-                </property>
-                <property name="bottomMargin">
-                 <number>0</number>
-                </property>
-                <property name="spacing">
-                 <number>0</number>
-                </property>
-                <item row="0" column="0">
-                 <widget class="QWidget" name="widget_6" native="true">
-                  <layout class="QGridLayout" name="gridLayout_12">
-                   <item row="1" column="0">
-                    <widget class="QLabel" name="label_18">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="text">
-                      <string>K</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="0">
-                    <widget class="QLabel" name="label_19">
-                     <property name="text">
-                      <string>D</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxNK">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>10.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="0" colspan="2">
-                    <widget class="QPushButton" name="pushButtonCfgSendNullspace">
-                     <property name="text">
-                      <string>Send</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="1">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBoxND">
-                     <property name="decimals">
-                      <number>4</number>
-                     </property>
-                     <property name="minimum">
-                      <double>-100.000000000000000</double>
-                     </property>
-                     <property name="maximum">
-                      <double>100.000000000000000</double>
-                     </property>
-                     <property name="value">
-                      <double>0.000000000000000</double>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="0" colspan="2">
-                    <widget class="QGroupBox" name="groupBox_8">
-                     <property name="title">
-                      <string>Attractor Joint Config</string>
-                     </property>
-                     <property name="checkable">
-                      <bool>true</bool>
-                     </property>
-                     <layout class="QGridLayout" name="gridLayout_15">
-                      <property name="leftMargin">
-                       <number>0</number>
-                      </property>
-                      <property name="topMargin">
-                       <number>0</number>
-                      </property>
-                      <property name="rightMargin">
-                       <number>0</number>
-                      </property>
-                      <property name="bottomMargin">
-                       <number>0</number>
-                      </property>
-                      <property name="spacing">
-                       <number>0</number>
-                      </property>
-                      <item row="0" column="0">
-                       <widget class="QWidget" name="widget_8" native="true">
-                        <layout class="QGridLayout" name="gridLayout_16">
-                         <item row="2" column="0">
-                          <layout class="QGridLayout" name="gridLayoutDefaultPoseL"/>
-                         </item>
-                         <item row="2" column="1">
-                          <layout class="QGridLayout" name="gridLayoutDefaultPoseR"/>
-                         </item>
-                         <item row="0" column="0">
-                          <widget class="QLabel" name="label_40">
-                           <property name="text">
-                            <string>Left</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="0" column="1">
-                          <widget class="QLabel" name="label_41">
-                           <property name="text">
-                            <string>Right</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="3" column="0" colspan="2">
-                          <widget class="QPushButton" name="pushButtonCfgSendDefaultPose">
-                           <property name="text">
-                            <string>Send</string>
-                           </property>
-                          </widget>
-                         </item>
-                        </layout>
-                       </widget>
-                      </item>
-                     </layout>
-                    </widget>
-                   </item>
-                  </layout>
-                 </widget>
-                </item>
-               </layout>
-              </widget>
-             </item>
-             <item row="6" column="0">
-              <widget class="QPushButton" name="pushButtonCfgSendAll">
-               <property name="text">
-                <string>Send</string>
-               </property>
-              </widget>
-             </item>
-            </layout>
-           </widget>
-          </item>
-         </layout>
-        </widget>
-       </item>
-       <item row="0" column="1">
-        <widget class="QPushButton" name="pushButtonCtrlActivate">
-         <property name="text">
-          <string>Activate</string>
-         </property>
-        </widget>
-       </item>
-       <item row="2" column="0" colspan="4">
-        <widget class="QGroupBox" name="groupBox_2">
-         <property name="title">
-          <string>Target</string>
-         </property>
-         <property name="checkable">
-          <bool>true</bool>
-         </property>
-         <layout class="QHBoxLayout" name="horizontalLayout_2">
-          <property name="spacing">
-           <number>0</number>
-          </property>
-          <property name="leftMargin">
-           <number>0</number>
-          </property>
-          <property name="topMargin">
-           <number>0</number>
-          </property>
-          <property name="rightMargin">
-           <number>0</number>
-          </property>
-          <property name="bottomMargin">
-           <number>0</number>
-          </property>
-          <item>
-           <widget class="QWidget" name="widget_2" native="true">
-            <layout class="QGridLayout" name="gridLayout_2">
-             <item row="0" column="5">
-              <widget class="QLabel" name="label_10">
-               <property name="text">
-                <string>Pitch</string>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="0">
-              <widget class="QLabel" name="label_2">
-               <property name="text">
-                <string>Velocity</string>
-               </property>
-              </widget>
-             </item>
-             <item row="1" column="0">
-              <widget class="QLabel" name="label">
-               <property name="sizePolicy">
-                <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-                 <horstretch>0</horstretch>
-                 <verstretch>0</verstretch>
-                </sizepolicy>
-               </property>
-               <property name="text">
-                <string>Target</string>
-               </property>
-              </widget>
-             </item>
-             <item row="1" column="1">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTX">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="1" column="5">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRY">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="6">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVRZ">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="0" column="3">
-              <widget class="QLabel" name="label_8">
-               <property name="text">
-                <string>Z</string>
-               </property>
-              </widget>
-             </item>
-             <item row="1" column="6">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRZ">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="3">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVTZ">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="0" column="4">
-              <widget class="QLabel" name="label_9">
-               <property name="text">
-                <string>Roll</string>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="4">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVRX">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="1" column="2">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTY">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="1" column="4">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRX">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="0" column="2">
-              <widget class="QLabel" name="label_4">
-               <property name="text">
-                <string>Y</string>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="1">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVTX">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="0" column="1">
-              <widget class="QLabel" name="label_3">
-               <property name="text">
-                <string>X</string>
-               </property>
-              </widget>
-             </item>
-             <item row="1" column="3">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTZ">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="2">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVTY">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="5">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVRY">
-               <property name="decimals">
-                <number>4</number>
-               </property>
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="value">
-                <double>0.000000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="0" column="6">
-              <widget class="QLabel" name="label_11">
-               <property name="text">
-                <string>Yaw</string>
-               </property>
-              </widget>
-             </item>
-             <item row="4" column="0" colspan="7">
-              <layout class="QHBoxLayout" name="horizontalLayout_3">
-               <item>
-                <widget class="QPushButton" name="pushButtonTargSend">
-                 <property name="text">
-                  <string>Send</string>
-                 </property>
-                </widget>
-               </item>
-               <item>
-                <widget class="QPushButton" name="pushButtonTargAdd">
-                 <property name="text">
-                  <string>Add Pose</string>
-                 </property>
-                </widget>
-               </item>
-               <item>
-                <widget class="Line" name="line">
-                 <property name="orientation">
-                  <enum>Qt::Vertical</enum>
-                 </property>
-                </widget>
-               </item>
-               <item>
-                <widget class="QPushButton" name="pushButtonReadCurrentPose">
-                 <property name="text">
-                  <string>Read current pose</string>
-                 </property>
-                </widget>
-               </item>
-              </layout>
-             </item>
-            </layout>
-           </widget>
-          </item>
-         </layout>
-        </widget>
-       </item>
-      </layout>
-     </widget>
-    </widget>
-   </item>
-  </layout>
- </widget>
- <resources/>
- <connections>
-  <connection>
-   <sender>groupBox</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>widget</receiver>
-   <slot>setVisible(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>55</x>
-     <y>50</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>152</x>
-     <y>169</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>groupBox_2</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>widget_2</receiver>
-   <slot>setVisible(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>49</x>
-     <y>1197</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>196</x>
-     <y>1220</y>
-    </hint>
-   </hints>
-  </connection>
- </connections>
-</ui>
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.cpp
deleted file mode 100644
index 92eb4c7083fde0208663cd7fc55b3de101fcb7ae..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::BimanualCartesianAdmittanceControllerGuiWidgetController
- * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date       2020
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include <string>
-
-#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
-#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "BimanualCartesianAdmittanceControllerGuiWidgetController.h"
-
-namespace armarx
-{
-    void clearLayout(QLayout* layout)
-    {
-        QLayoutItem* item;
-        while ((item = layout->takeAt(0)))
-        {
-            if (item->layout())
-            {
-                clearLayout(item->layout());
-                delete item->layout();
-            }
-            if (item->widget())
-            {
-                delete item->widget();
-            }
-            delete item;
-        }
-    }
-
-    BimanualCartesianAdmittanceControllerGuiWidgetController::BimanualCartesianAdmittanceControllerGuiWidgetController() :
-        NJointControllerGuiPluginBase("NJointBimanualCartesianAdmittanceController")
-    {
-        _ui.setupUi(getWidget());
-        connectCreateAcivateDeactivateDelete(
-            _ui.pushButtonCtrlCreate,
-            _ui.pushButtonCtrlActivate,
-            _ui.pushButtonCtrlDeactivate,
-            _ui.pushButtonCtrlDelete
-        );
-        using T = BimanualCartesianAdmittanceControllerGuiWidgetController;
-        connect(_ui.pushButtonReadCurrentPose,      &QPushButton::clicked, this, &T::on_pushButtonReadCurrentPose_clicked);
-        connect(_ui.pushButtonTargAdd,              &QPushButton::clicked, this, &T::on_pushButtonTargAdd_clicked);
-        connect(_ui.pushButtonCfgSendDefaultPose,   &QPushButton::clicked, this, &T::on_pushButtonCfgSendDefaultPose_clicked);
-        connect(_ui.pushButtonCfgSendNullspace,     &QPushButton::clicked, this, &T::on_pushButtonCfgSendNullspace_clicked);
-        connect(_ui.pushButtonCfgSendImpedance,     &QPushButton::clicked, this, &T::on_pushButtonCfgSendImpedance_clicked);
-        connect(_ui.pushButtonCfgSendAdmittance,    &QPushButton::clicked, this, &T::on_pushButtonCfgSendAdmittance_clicked);
-        connect(_ui.pushButtonCfgSendForce,         &QPushButton::clicked, this, &T::on_pushButtonCfgSendForce_clicked);
-        connect(_ui.pushButtonCfgSendAll,           &QPushButton::clicked, this, &T::on_pushButtonCfgSendAll_clicked);
-        connect(_ui.pushButtonTargSend,             &QPushButton::clicked, this, &T::on_pushButtonTargSend_clicked);
-    }
-
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::setupGuiAfterConnect()
-    {
-        //fill rns combo box
-        {
-            const auto fill = [&](auto rnsname, auto & lay, auto & sp)
-            {
-                static const std::map<std::string, double> defaults
-                {
-                    ///TODO
-                };
-
-                clearLayout(lay);
-                int i = 0;
-                for (const auto& rn : _robot->getRobotNodeSet(rnsname)->getAllRobotNodes())
-                {
-                    const auto&& n = rn->getName();
-                    lay->addWidget(new QLabel{QString::fromStdString(n)}, i, 0);
-                    auto b = new QDoubleSpinBox;
-                    sp.addWidget(b);
-                    lay->addWidget(b, i, 1);
-                    const auto lo = rn->getJointLimitLow();
-                    const auto hi = rn->getJointLimitHigh();
-                    b->setMinimum(lo);
-                    b->setMaximum(hi);
-                    b->setValue(defaults.count(n) ? defaults.at(n) : (lo + hi) / 2);
-                    ++i;
-                }
-            };
-            fill("LeftArm", _ui.gridLayoutDefaultPoseL, _desiredJointValuesLeft);
-            fill("RightArm", _ui.gridLayoutDefaultPoseR, _desiredJointValuesRight);
-        }
-    }
-}
-//read config
-namespace armarx
-{
-    std::array<Ice::FloatSeq, 2>
-    BimanualCartesianAdmittanceControllerGuiWidgetController::readDesiredJointCFG() const
-    {
-        return
-        {
-            _desiredJointValuesLeft.get<std::vector<float>>(),
-                    _desiredJointValuesRight.get<std::vector<float>>()
-        };
-    }
-    detail::NJBmanCartAdmCtrl::Nullspace
-    BimanualCartesianAdmittanceControllerGuiWidgetController::readNullspaceCFG() const
-    {
-        detail::NJBmanCartAdmCtrl::Nullspace c;
-        c.k = _ui.doubleSpinBoxNK->value();
-        c.d = _ui.doubleSpinBoxND->value();
-        const auto arms = readDesiredJointCFG();
-        c.desiredJointValuesLeft = arms.at(0);
-        c.desiredJointValuesRight = arms.at(1);
-        return c;
-    }
-    std::array<detail::NJBmanCartAdmCtrl::Impedance, 2>
-    BimanualCartesianAdmittanceControllerGuiWidgetController::readImpedanceCFG() const
-    {
-        detail::NJBmanCartAdmCtrl::Impedance l;
-        detail::NJBmanCartAdmCtrl::Impedance r;
-
-        l.KpXYZ(0) = _ui.doubleSpinBoxIPTXL->value();
-        l.KpXYZ(1) = _ui.doubleSpinBoxIPTYL->value();
-        l.KpXYZ(2) = _ui.doubleSpinBoxIPTZL->value();
-
-        l.KpRPY(0) = _ui.doubleSpinBoxIPRXL->value();
-        l.KpRPY(1) = _ui.doubleSpinBoxIPRYL->value();
-        l.KpRPY(2) = _ui.doubleSpinBoxIPRZL->value();
-
-        l.KdXYZ(0) = _ui.doubleSpinBoxIDTXL->value();
-        l.KdXYZ(1) = _ui.doubleSpinBoxIDTYL->value();
-        l.KdXYZ(2) = _ui.doubleSpinBoxIDTZL->value();
-
-        l.KdRPY(0) = _ui.doubleSpinBoxIDRXL->value();
-        l.KdRPY(1) = _ui.doubleSpinBoxIDRYL->value();
-        l.KdRPY(2) = _ui.doubleSpinBoxIDRZL->value();
-
-        r.KpXYZ(0) = _ui.doubleSpinBoxIPTXR->value();
-        r.KpXYZ(1) = _ui.doubleSpinBoxIPTYR->value();
-        r.KpXYZ(2) = _ui.doubleSpinBoxIPTZR->value();
-
-        r.KpRPY(0) = _ui.doubleSpinBoxIPRXR->value();
-        r.KpRPY(1) = _ui.doubleSpinBoxIPRYR->value();
-        r.KpRPY(2) = _ui.doubleSpinBoxIPRZR->value();
-
-        r.KdXYZ(0) = _ui.doubleSpinBoxIDTXR->value();
-        r.KdXYZ(1) = _ui.doubleSpinBoxIDTYR->value();
-        r.KdXYZ(2) = _ui.doubleSpinBoxIDTZR->value();
-
-        r.KdRPY(0) = _ui.doubleSpinBoxIDRXR->value();
-        r.KdRPY(1) = _ui.doubleSpinBoxIDRYR->value();
-        r.KdRPY(2) = _ui.doubleSpinBoxIDRZR->value();
-
-        return {l, r};
-    }
-    std::array<detail::NJBmanCartAdmCtrl::Force, 2>
-    BimanualCartesianAdmittanceControllerGuiWidgetController::readForceCFG() const
-    {
-        detail::NJBmanCartAdmCtrl::Force l;
-        detail::NJBmanCartAdmCtrl::Force r;
-
-        l.wrenchXYZ(0) = _ui.doubleSpinBoxFWTXL->value();
-        l.wrenchXYZ(1) = _ui.doubleSpinBoxFWTYL->value();
-        l.wrenchXYZ(2) = _ui.doubleSpinBoxFWTZL->value();
-
-        l.wrenchRPY(0) = _ui.doubleSpinBoxFWRXL->value();
-        l.wrenchRPY(1) = _ui.doubleSpinBoxFWRYL->value();
-        l.wrenchRPY(2) = _ui.doubleSpinBoxFWRZL->value();
-
-        l.mass = _ui.doubleSpinBoxFML->value();
-
-        l.offsetForce(0) = _ui.doubleSpinBoxFOFXL->value();
-        l.offsetForce(1) = _ui.doubleSpinBoxFOFYL->value();
-        l.offsetForce(2) = _ui.doubleSpinBoxFOFZL->value();
-
-        l.offsetTorque(0) = _ui.doubleSpinBoxFOTXL->value();
-        l.offsetTorque(1) = _ui.doubleSpinBoxFOTYL->value();
-        l.offsetTorque(2) = _ui.doubleSpinBoxFOTZL->value();
-
-        l.forceThreshold(0) = _ui.doubleSpinBoxFFTXL->value();
-        l.forceThreshold(1) = _ui.doubleSpinBoxFFTYL->value();
-        l.forceThreshold(2) = _ui.doubleSpinBoxFFTZL->value();
-
-        r.wrenchXYZ(0) = _ui.doubleSpinBoxFWTXR->value();
-        r.wrenchXYZ(1) = _ui.doubleSpinBoxFWTYR->value();
-        r.wrenchXYZ(2) = _ui.doubleSpinBoxFWTZR->value();
-
-        r.wrenchRPY(0) = _ui.doubleSpinBoxFWRXR->value();
-        r.wrenchRPY(1) = _ui.doubleSpinBoxFWRYR->value();
-        r.wrenchRPY(2) = _ui.doubleSpinBoxFWRZR->value();
-
-        r.mass = _ui.doubleSpinBoxFMR->value();
-
-        r.offsetForce(0) = _ui.doubleSpinBoxFOFXR->value();
-        r.offsetForce(1) = _ui.doubleSpinBoxFOFYR->value();
-        r.offsetForce(2) = _ui.doubleSpinBoxFOFZR->value();
-
-        r.offsetTorque(0) = _ui.doubleSpinBoxFOTXR->value();
-        r.offsetTorque(1) = _ui.doubleSpinBoxFOTYR->value();
-        r.offsetTorque(2) = _ui.doubleSpinBoxFOTZR->value();
-
-        r.forceThreshold(0) = _ui.doubleSpinBoxFFTXR->value();
-        r.forceThreshold(1) = _ui.doubleSpinBoxFFTYR->value();
-        r.forceThreshold(2) = _ui.doubleSpinBoxFFTZR->value();
-        return {l, r};
-    }
-    detail::NJBmanCartAdmCtrl::Admittance
-    BimanualCartesianAdmittanceControllerGuiWidgetController::readAdmittanceCFG() const
-    {
-        detail::NJBmanCartAdmCtrl::Admittance c;
-
-        c.KpXYZ(0) = _ui.doubleSpinBoxAPTX->value();
-        c.KpXYZ(1) = _ui.doubleSpinBoxAPTY->value();
-        c.KpXYZ(2) = _ui.doubleSpinBoxAPTZ->value();
-
-        c.KpRPY(0) = _ui.doubleSpinBoxAPRX->value();
-        c.KpRPY(1) = _ui.doubleSpinBoxAPRY->value();
-        c.KpRPY(2) = _ui.doubleSpinBoxAPRZ->value();
-
-        c.KdXYZ(0) = _ui.doubleSpinBoxADTX->value();
-        c.KdXYZ(1) = _ui.doubleSpinBoxADTY->value();
-        c.KdXYZ(2) = _ui.doubleSpinBoxADTZ->value();
-
-        c.KdRPY(0) = _ui.doubleSpinBoxADRX->value();
-        c.KdRPY(1) = _ui.doubleSpinBoxADRY->value();
-        c.KdRPY(2) = _ui.doubleSpinBoxADRZ->value();
-
-        c.KmXYZ(0) = _ui.doubleSpinBoxAMTX->value();
-        c.KmXYZ(1) = _ui.doubleSpinBoxAMTY->value();
-        c.KmXYZ(2) = _ui.doubleSpinBoxAMTZ->value();
-
-        c.KmRPY(0) = _ui.doubleSpinBoxAMRX->value();
-        c.KmRPY(1) = _ui.doubleSpinBoxAMRY->value();
-        c.KmRPY(2) = _ui.doubleSpinBoxAMRZ->value();
-
-        return c;
-    }
-    NJointControllerConfigPtr
-    BimanualCartesianAdmittanceControllerGuiWidgetController::readFullCFG() const
-    {
-        NJointBimanualCartesianAdmittanceControllerConfigPtr c =
-            new NJointBimanualCartesianAdmittanceControllerConfig;
-        c->kinematicChainRight = "RightArm";
-        c->kinematicChainLeft = "LeftArm";
-        c->ftSensorRight = "FT R";
-        c->ftSensorLeft = "FT L";
-        c->ftSensorRightFrame = "ArmR8_Wri2";
-        c->ftSensorLeftFrame = "ArmL8_Wri2";
-        c->box.width = _ui.doubleSpinBoxOBoxw->value();
-        c->filterCoeff = _ui.doubleSpinBoxOFiltCoeff->value();
-        c->torqueLimit = _ui.doubleSpinBoxOTorqueLim->value();
-        c->ftCalibrationTime = _ui.doubleSpinBoxOCalibTime->value();
-        c->nullspace = readNullspaceCFG();
-        c->admittanceObject = readAdmittanceCFG();
-        const auto f = readForceCFG();
-        c->forceLeft = f.at(0);
-        c->forceRight = f.at(1);
-        const auto i = readImpedanceCFG();
-        c->impedanceLeft = i.at(0);
-        c->impedanceRight = i.at(1);
-        return NJointControllerConfigPtr::dynamicCast(c);
-    }
-    std::array<Eigen::Vector3f, 2>
-    BimanualCartesianAdmittanceControllerGuiWidgetController::readPosTarg() const
-    {
-        Eigen::Vector3f xyz;
-        Eigen::Vector3f rpy;
-
-        xyz(0) = _ui.doubleSpinBoxTargTX->value();
-        xyz(1) = _ui.doubleSpinBoxTargTY->value();
-        xyz(2) = _ui.doubleSpinBoxTargTZ->value();
-
-        rpy(0) = _ui.doubleSpinBoxTargRX->value();
-        rpy(1) = _ui.doubleSpinBoxTargRY->value();
-        rpy(2) = _ui.doubleSpinBoxTargRZ->value();
-
-        return {xyz, rpy};
-    }
-    std::array<Eigen::Vector3f, 2>
-    BimanualCartesianAdmittanceControllerGuiWidgetController::readVelTarg() const
-    {
-        Eigen::Vector3f xyz;
-        Eigen::Vector3f rpy;
-
-        xyz(0) = _ui.doubleSpinBoxTargVTX->value();
-        xyz(1) = _ui.doubleSpinBoxTargVTY->value();
-        xyz(2) = _ui.doubleSpinBoxTargVTZ->value();
-
-        rpy(0) = _ui.doubleSpinBoxTargVRX->value();
-        rpy(1) = _ui.doubleSpinBoxTargVRY->value();
-        rpy(2) = _ui.doubleSpinBoxTargVRZ->value();
-
-        return {xyz, rpy};
-    }
-}
-//push buttons
-namespace armarx
-{
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendAll_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        _controller->setConfig(NJointBimanualCartesianAdmittanceControllerConfigPtr::dynamicCast(readFullCFG()));
-    }
-
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendForce_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        const auto c = readForceCFG();
-        _controller->setForceConfig(c.at(0), c.at(1));
-    }
-
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendAdmittance_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        _controller->setAdmittanceConfig(readAdmittanceCFG());
-    }
-
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendImpedance_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        const auto c = readImpedanceCFG();
-        _controller->setImpedanceConfig(c.at(0), c.at(1));
-    }
-
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendNullspace_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        _controller->setNullspaceConfig(readNullspaceCFG());
-    }
-
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendDefaultPose_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        const auto c = readDesiredJointCFG();
-        _controller->setDesiredJointValuesLeft(c.at(0));
-        _controller->setDesiredJointValuesRight(c.at(1));
-    }
-
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonTargSend_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        const auto t = readPosTarg();
-        const auto v = readVelTarg();
-        const Eigen::Matrix4f m = simox::math::pos_rpy_to_mat4f(t.at(0), t.at(1));
-        _controller->setBoxPoseAndVelocity(m, v.at(0), v.at(1));
-    }
-
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonTargAdd_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        const auto t = readPosTarg();
-        const Eigen::Matrix4f m = simox::math::pos_rpy_to_mat4f(t.at(0), t.at(1));
-        _controller->moveBoxPose(m);
-    }
-
-    void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonReadCurrentPose_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        const Eigen::Matrix4f m = _controller->getBoxPose();
-        const auto rpy = simox::math::mat4f_to_rpy(m);
-        _ui.doubleSpinBoxTargTX->setValue(m(0, 3));
-        _ui.doubleSpinBoxTargTY->setValue(m(1, 3));
-        _ui.doubleSpinBoxTargTZ->setValue(m(2, 3));
-        _ui.doubleSpinBoxTargRX->setValue(rpy(0));
-        _ui.doubleSpinBoxTargRY->setValue(rpy(1));
-        _ui.doubleSpinBoxTargRZ->setValue(rpy(2));
-    }
-}
-
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.h b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.h
deleted file mode 100644
index cbcbb472458106123a806e8c67cae865344ef883..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::gui-plugins::BimanualCartesianAdmittanceControllerGuiWidgetController
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2020
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.h>
-#include <RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/ui_BimanualCartesianAdmittanceControllerGuiWidget.h>
-
-namespace armarx
-{
-    /**
-    \page RobotAPI-GuiPlugins-BimanualCartesianAdmittanceControllerGui BimanualCartesianAdmittanceControllerGui
-    \brief The BimanualCartesianAdmittanceControllerGui allows visualizing ...
-
-    \image html BimanualCartesianAdmittanceControllerGui.png
-    The user can
-
-    API Documentation \ref BimanualCartesianAdmittanceControllerGuiWidgetController
-
-    \see BimanualCartesianAdmittanceControllerGuiGuiPlugin
-    */
-
-    /**
-     * \class BimanualCartesianAdmittanceControllerGuiWidgetController
-     * \brief BimanualCartesianAdmittanceControllerGuiWidgetController brief one line description
-     *
-     * Detailed description
-     */
-    class ARMARXCOMPONENT_IMPORT_EXPORT
-        BimanualCartesianAdmittanceControllerGuiWidgetController:
-        public NJointControllerGuiPluginBase <
-        BimanualCartesianAdmittanceControllerGuiWidgetController,
-        NJointBimanualCartesianAdmittanceControllerInterfacePrx
-        >
-    {
-        Q_OBJECT
-
-    public:
-        explicit BimanualCartesianAdmittanceControllerGuiWidgetController();
-
-        /**
-         * Returns the Widget name displayed in the ArmarXGui to create an
-         * instance of this class.
-         */
-        static QString GetWidgetName()
-        {
-            return "RobotControl.NJointControllers.BimanualCartesianAdmittanceController";
-        }
-
-    private slots:
-        void on_pushButtonReadCurrentPose_clicked();
-        void on_pushButtonTargAdd_clicked();
-        void on_pushButtonCfgSendDefaultPose_clicked();
-        void on_pushButtonCfgSendNullspace_clicked();
-        void on_pushButtonCfgSendImpedance_clicked();
-        void on_pushButtonCfgSendAdmittance_clicked();
-        void on_pushButtonCfgSendForce_clicked();
-        void on_pushButtonCfgSendAll_clicked();
-        void on_pushButtonTargSend_clicked();
-
-    private:
-        void setupGuiAfterConnect() override;
-
-        std::array<Ice::FloatSeq, 2> readDesiredJointCFG() const;
-        detail::NJBmanCartAdmCtrl::Nullspace readNullspaceCFG() const;
-        std::array<detail::NJBmanCartAdmCtrl::Impedance, 2> readImpedanceCFG() const;
-        std::array<detail::NJBmanCartAdmCtrl::Force, 2> readForceCFG() const;
-        detail::NJBmanCartAdmCtrl::Admittance readAdmittanceCFG() const;
-        NJointControllerConfigPtr readFullCFG() const override;
-        std::array<Eigen::Vector3f, 2> readPosTarg() const;
-        std::array<Eigen::Vector3f, 2> readVelTarg() const;
-    private:
-        Ui::BimanualCartesianAdmittanceControllerGuiWidget  _ui;
-        SpinBoxToVector<QDoubleSpinBox>                     _desiredJointValuesLeft;
-        SpinBoxToVector<QDoubleSpinBox>                     _desiredJointValuesRight;
-    };
-}
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/CMakeLists.txt
deleted file mode 100644
index df858aec05921a3bb2c749d6965529676a0265c7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/CMakeLists.txt
+++ /dev/null
@@ -1,18 +0,0 @@
-armarx_set_target("BimanualCartesianAdmittanceControllerGuiGuiPlugin")
-armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
-
-set(SOURCES
-    BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp 
-    BimanualCartesianAdmittanceControllerGuiWidgetController.cpp
-)
-set(HEADERS
-    BimanualCartesianAdmittanceControllerGuiGuiPlugin.h
-    BimanualCartesianAdmittanceControllerGuiWidgetController.h
-)
-set(GUI_MOC_HDRS ${HEADERS})
-set(GUI_UIS BimanualCartesianAdmittanceControllerGuiWidget.ui)
-set(COMPONENT_LIBS NJointControllerGuiPluginUtility)
-
-if(ArmarXGui_FOUND)
-    armarx_gui_library(BimanualCartesianAdmittanceControllerGuiGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index 48c9062175974a656810cb82721bdca203b2769c..d61010d07592b01858adfaea525eedba338256bb 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -11,13 +11,9 @@ add_subdirectory(RobotUnitPlugin)
 add_subdirectory(HomogeneousMatrixCalculator)
 
 add_subdirectory(GuiHealthClient)
-add_subdirectory(CartesianWaypointControlGui)
 add_subdirectory(DebugDrawerGuiPlugin)
 add_subdirectory(ArViz)
-add_subdirectory(CartesianNaturalPositionController)
 add_subdirectory(ObjectPoseGui)
-add_subdirectory(CartesianImpedanceController)
-add_subdirectory(BimanualCartesianAdmittanceControllerGui)
 
 add_subdirectory(ArMemMemoryViewer)
 add_subdirectory(ArVizDrawerGui)
@@ -28,4 +24,4 @@ add_subdirectory(BoxToGraspCandidates)
 
 
 
-add_subdirectory(DebugRobotUnitDataStreaming)
\ No newline at end of file
+add_subdirectory(DebugRobotUnitDataStreaming)
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CMakeLists.txt b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CMakeLists.txt
deleted file mode 100644
index 302265b78a951820285f776cf9ad4d39b08d1dda..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CMakeLists.txt
+++ /dev/null
@@ -1,18 +0,0 @@
-armarx_set_target("CartesianImpedanceControllerGuiPlugin")
-armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
-
-set(SOURCES 
-    CartesianImpedanceControllerGuiPlugin.cpp
-    CartesianImpedanceControllerWidgetController.cpp
-)
-set(HEADERS 
-    CartesianImpedanceControllerGuiPlugin.h
-    CartesianImpedanceControllerWidgetController.h
-)
-set(GUI_MOC_HDRS ${HEADERS})
-set(GUI_UIS CartesianImpedanceControllerWidget.ui)
-set(COMPONENT_LIBS NJointControllerGuiPluginUtility RobotAPINJointControllerWidgets)
-
-if(ArmarXGui_FOUND)
-    armarx_gui_library(CartesianImpedanceControllerGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.cpp
deleted file mode 100644
index 5330f27cafcb9d3a401b3ec10d1ed0d625f45962..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::CartesianImpedanceControllerGuiPlugin
- * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date       2020
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "CartesianImpedanceControllerGuiPlugin.h"
-
-#include "CartesianImpedanceControllerWidgetController.h"
-
-namespace armarx
-{
-    CartesianImpedanceControllerGuiPlugin::CartesianImpedanceControllerGuiPlugin()
-    {
-        addWidget < CartesianImpedanceControllerWidgetController > ();
-    }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.h b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.h
deleted file mode 100644
index 059b3a107c9d3f08aa39ecc144c21cc5bbeae1b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::CartesianImpedanceController
- * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date       2020
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-
-namespace armarx
-{
-    /**
-     * \class CartesianImpedanceControllerGuiPlugin
-     * \ingroup ArmarXGuiPlugins
-     * \brief CartesianImpedanceControllerGuiPlugin brief description
-     *
-     * Detailed description
-     */
-    class ARMARXCOMPONENT_IMPORT_EXPORT CartesianImpedanceControllerGuiPlugin:
-        public armarx::ArmarXGuiPlugin
-    {
-        Q_OBJECT
-        Q_INTERFACES(ArmarXGuiInterface)
-        Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
-    public:
-        /**
-         * All widgets exposed by this plugin are added in the constructor
-         * via calls to addWidget()
-         */
-        CartesianImpedanceControllerGuiPlugin();
-    };
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui
deleted file mode 100644
index 890f754e5a2e26d8c22226868c5bcbbc98ee5a98..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui
+++ /dev/null
@@ -1,549 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>CartesianImpedanceControllerWidget</class>
- <widget class="QWidget" name="CartesianImpedanceControllerWidget">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>612</width>
-    <height>594</height>
-   </rect>
-  </property>
-  <property name="windowTitle">
-   <string>CartesianImpedanceControllerWidget</string>
-  </property>
-  <layout class="QGridLayout" name="gridLayout">
-   <item row="0" column="0">
-    <widget class="QScrollArea" name="scrollArea">
-     <property name="widgetResizable">
-      <bool>true</bool>
-     </property>
-     <widget class="QWidget" name="scrollAreaWidgetContents">
-      <property name="geometry">
-       <rect>
-        <x>0</x>
-        <y>0</y>
-        <width>592</width>
-        <height>574</height>
-       </rect>
-      </property>
-      <layout class="QVBoxLayout" name="verticalLayout">
-       <item>
-        <widget class="QWidget" name="widgetRNSSelect" native="true">
-         <layout class="QVBoxLayout" name="verticalLayout_4">
-          <property name="leftMargin">
-           <number>0</number>
-          </property>
-          <property name="topMargin">
-           <number>0</number>
-          </property>
-          <property name="rightMargin">
-           <number>0</number>
-          </property>
-          <property name="bottomMargin">
-           <number>0</number>
-          </property>
-          <item>
-           <layout class="QGridLayout" name="gridLayoutRNS">
-            <item row="0" column="2">
-             <layout class="QHBoxLayout" name="horizontalLayout">
-              <item>
-               <widget class="QRadioButton" name="radioButtonSelectLeft">
-                <property name="text">
-                 <string>Left</string>
-                </property>
-                <property name="checked">
-                 <bool>false</bool>
-                </property>
-               </widget>
-              </item>
-              <item>
-               <widget class="QRadioButton" name="radioButtonSelectRight">
-                <property name="text">
-                 <string>Right</string>
-                </property>
-                <property name="checked">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
-              <item>
-               <widget class="QRadioButton" name="radioButtonSelectRNS">
-                <property name="text">
-                 <string>RNS</string>
-                </property>
-               </widget>
-              </item>
-             </layout>
-            </item>
-            <item row="1" column="2">
-             <widget class="QComboBox" name="comboBoxRNS">
-              <property name="enabled">
-               <bool>false</bool>
-              </property>
-             </widget>
-            </item>
-            <item row="1" column="0">
-             <widget class="QLabel" name="label">
-              <property name="sizePolicy">
-               <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
-                <horstretch>0</horstretch>
-                <verstretch>0</verstretch>
-               </sizepolicy>
-              </property>
-              <property name="text">
-               <string>RNS</string>
-              </property>
-             </widget>
-            </item>
-            <item row="0" column="0">
-             <widget class="QLabel" name="label_3">
-              <property name="text">
-               <string>Select</string>
-              </property>
-             </widget>
-            </item>
-           </layout>
-          </item>
-         </layout>
-        </widget>
-       </item>
-       <item>
-        <layout class="QHBoxLayout" name="horizontalLayout_2">
-         <item>
-          <widget class="QPushButton" name="pushButtonCreate">
-           <property name="text">
-            <string>Create</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QPushButton" name="pushButtonActivate">
-           <property name="text">
-            <string>Activate</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QPushButton" name="pushButtonDeactivate">
-           <property name="text">
-            <string>Deactivate</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QPushButton" name="pushButtonDelete">
-           <property name="text">
-            <string>Delete</string>
-           </property>
-          </widget>
-         </item>
-        </layout>
-       </item>
-       <item>
-        <widget class="QGroupBox" name="groupBox_2">
-         <property name="title">
-          <string>Settings</string>
-         </property>
-         <property name="checkable">
-          <bool>true</bool>
-         </property>
-         <layout class="QVBoxLayout" name="verticalLayout_2">
-          <property name="spacing">
-           <number>0</number>
-          </property>
-          <property name="leftMargin">
-           <number>0</number>
-          </property>
-          <property name="topMargin">
-           <number>0</number>
-          </property>
-          <property name="rightMargin">
-           <number>0</number>
-          </property>
-          <property name="bottomMargin">
-           <number>0</number>
-          </property>
-          <item>
-           <widget class="QWidget" name="widget" native="true">
-            <layout class="QGridLayout" name="gridLayoutSettings">
-             <property name="leftMargin">
-              <number>0</number>
-             </property>
-             <property name="topMargin">
-              <number>0</number>
-             </property>
-             <property name="rightMargin">
-              <number>0</number>
-             </property>
-             <property name="bottomMargin">
-              <number>0</number>
-             </property>
-            </layout>
-           </widget>
-          </item>
-         </layout>
-        </widget>
-       </item>
-       <item>
-        <widget class="QGroupBox" name="groupBox">
-         <property name="title">
-          <string>Target</string>
-         </property>
-         <property name="checkable">
-          <bool>true</bool>
-         </property>
-         <layout class="QVBoxLayout" name="verticalLayout_3">
-          <property name="spacing">
-           <number>0</number>
-          </property>
-          <property name="leftMargin">
-           <number>0</number>
-          </property>
-          <property name="topMargin">
-           <number>0</number>
-          </property>
-          <property name="rightMargin">
-           <number>0</number>
-          </property>
-          <property name="bottomMargin">
-           <number>0</number>
-          </property>
-          <item>
-           <widget class="QWidget" name="widgetTarget" native="true">
-            <layout class="QGridLayout" name="gridLayout_2">
-             <item row="1" column="1">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRX">
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="singleStep">
-                <double>0.010000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="1" column="3">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRZ">
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="singleStep">
-                <double>0.010000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="1" column="2">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRY">
-               <property name="minimum">
-                <double>-100.000000000000000</double>
-               </property>
-               <property name="maximum">
-                <double>100.000000000000000</double>
-               </property>
-               <property name="singleStep">
-                <double>0.010000000000000</double>
-               </property>
-              </widget>
-             </item>
-             <item row="0" column="3">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTZ"/>
-             </item>
-             <item row="2" column="0" colspan="4">
-              <layout class="QGridLayout" name="gridLayout_3">
-               <item row="1" column="0">
-                <widget class="QPushButton" name="pushButtonTargGet">
-                 <property name="text">
-                  <string>Set to current</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="2">
-                <widget class="QRadioButton" name="radioButtonVisuAdd">
-                 <property name="text">
-                  <string>Add Pose</string>
-                 </property>
-                 <property name="checked">
-                  <bool>false</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="0">
-                <widget class="QLabel" name="label_2">
-                 <property name="text">
-                  <string>Visu</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="1">
-                <widget class="QRadioButton" name="radioButtonVisuSet">
-                 <property name="text">
-                  <string>Set Pose</string>
-                 </property>
-                 <property name="checked">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="1">
-                <widget class="QPushButton" name="pushButtonTargSet">
-                 <property name="text">
-                  <string>Set Pose</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="2">
-                <widget class="QPushButton" name="pushButtonTargAdd">
-                 <property name="enabled">
-                  <bool>false</bool>
-                 </property>
-                 <property name="text">
-                  <string>Add Pose</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="2">
-                <widget class="QCheckBox" name="checkBoxEnableAddPose">
-                 <property name="text">
-                  <string>enable add pose</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="1">
-                <widget class="QCheckBox" name="checkBoxAutoUpdatePose">
-                 <property name="text">
-                  <string>Auto set pose</string>
-                 </property>
-                </widget>
-               </item>
-              </layout>
-             </item>
-             <item row="0" column="2">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTY"/>
-             </item>
-             <item row="0" column="0">
-              <widget class="QLabel" name="label_7">
-               <property name="sizePolicy">
-                <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-                 <horstretch>0</horstretch>
-                 <verstretch>0</verstretch>
-                </sizepolicy>
-               </property>
-               <property name="text">
-                <string>XYZ</string>
-               </property>
-              </widget>
-             </item>
-             <item row="0" column="1">
-              <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTX"/>
-             </item>
-             <item row="1" column="0">
-              <widget class="QLabel" name="label_8">
-               <property name="text">
-                <string>RPY</string>
-               </property>
-              </widget>
-             </item>
-            </layout>
-           </widget>
-          </item>
-         </layout>
-        </widget>
-       </item>
-       <item>
-        <widget class="QGroupBox" name="groupBoxAdvanced">
-         <property name="title">
-          <string>Advanced</string>
-         </property>
-         <property name="checkable">
-          <bool>true</bool>
-         </property>
-         <layout class="QGridLayout" name="gridLayout_4">
-          <property name="leftMargin">
-           <number>0</number>
-          </property>
-          <property name="topMargin">
-           <number>0</number>
-          </property>
-          <property name="rightMargin">
-           <number>0</number>
-          </property>
-          <property name="bottomMargin">
-           <number>0</number>
-          </property>
-          <item row="0" column="0">
-           <widget class="QWidget" name="widget_3" native="true">
-            <layout class="QGridLayout" name="gridLayout_6">
-             <item row="1" column="0">
-              <widget class="QCheckBox" name="checkBoxLockZ">
-               <property name="text">
-                <string>Lock Z</string>
-               </property>
-              </widget>
-             </item>
-             <item row="0" column="0">
-              <widget class="QCheckBox" name="checkBoxLockY">
-               <property name="sizePolicy">
-                <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
-                 <horstretch>0</horstretch>
-                 <verstretch>0</verstretch>
-                </sizepolicy>
-               </property>
-               <property name="text">
-                <string>Lock Y</string>
-               </property>
-              </widget>
-             </item>
-             <item row="0" column="1">
-              <widget class="QComboBox" name="comboBoxLockY"/>
-             </item>
-             <item row="1" column="1">
-              <widget class="QComboBox" name="comboBoxLockZ"/>
-             </item>
-            </layout>
-           </widget>
-          </item>
-         </layout>
-        </widget>
-       </item>
-       <item>
-        <spacer name="verticalSpacer">
-         <property name="orientation">
-          <enum>Qt::Vertical</enum>
-         </property>
-         <property name="sizeHint" stdset="0">
-          <size>
-           <width>20</width>
-           <height>40</height>
-          </size>
-         </property>
-        </spacer>
-       </item>
-      </layout>
-     </widget>
-    </widget>
-   </item>
-  </layout>
- </widget>
- <resources/>
- <connections>
-  <connection>
-   <sender>groupBox_2</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>widget</receiver>
-   <slot>setVisible(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>103</x>
-     <y>121</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>207</x>
-     <y>139</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>groupBox</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>widgetTarget</receiver>
-   <slot>setVisible(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>23</x>
-     <y>233</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>232</x>
-     <y>263</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>groupBoxAdvanced</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>widget_3</receiver>
-   <slot>setVisible(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>328</x>
-     <y>346</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>334</x>
-     <y>362</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>radioButtonSelectRNS</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>comboBoxRNS</receiver>
-   <slot>setEnabled(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>590</x>
-     <y>41</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>281</x>
-     <y>68</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>radioButtonSelectRNS</sender>
-   <signal>toggled(bool)</signal>
-   <receiver>comboBoxRNS</receiver>
-   <slot>setEnabled(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>590</x>
-     <y>40</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>317</x>
-     <y>69</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>groupBoxAdvanced</sender>
-   <signal>toggled(bool)</signal>
-   <receiver>widget_3</receiver>
-   <slot>setVisible(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>73</x>
-     <y>336</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>179</x>
-     <y>357</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>checkBoxEnableAddPose</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>pushButtonTargAdd</receiver>
-   <slot>setEnabled(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>449</x>
-     <y>281</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>461</x>
-     <y>259</y>
-    </hint>
-   </hints>
-  </connection>
- </connections>
-</ui>
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp
deleted file mode 100644
index 6b34ac81d1d2bd48e8f6bdaae2cbf2bd2b711b26..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp
+++ /dev/null
@@ -1,316 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::CartesianImpedanceControllerWidgetController
- * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date       2020
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include <string>
-
-#include <SimoxUtility/math/convert/mat4f_to_pos.h>
-#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
-#include <SimoxUtility/math/convert/rpy_to_quat.h>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "CartesianImpedanceControllerWidgetController.h"
-
-namespace armarx
-{
-    CartesianImpedanceControllerWidgetController::CartesianImpedanceControllerWidgetController() :
-        NJointControllerGuiPluginBase("NJointTaskSpaceImpedanceController")
-    {
-        _ui.setupUi(getWidget());
-        connectCreateAcivateDeactivateDelete(
-            _ui.pushButtonCreate,
-            _ui.pushButtonActivate,
-            _ui.pushButtonDeactivate,
-            _ui.pushButtonDelete
-        );
-        _settings = new CartesianImpedanceControllerConfigWidget(getWidget());
-        _ui.gridLayoutSettings->addWidget(_settings);
-
-        _targ.setXYZWidgets(
-            _ui.doubleSpinBoxTargTX,
-            _ui.doubleSpinBoxTargTY,
-            _ui.doubleSpinBoxTargTZ
-        );
-
-        _targ.setRPYWidgets(
-            _ui.doubleSpinBoxTargRX,
-            _ui.doubleSpinBoxTargRY,
-            _ui.doubleSpinBoxTargRZ
-        );
-
-        _targ.setDefaultLimits();
-        _timer = startTimer(10);
-    }
-    CartesianImpedanceControllerWidgetController::~CartesianImpedanceControllerWidgetController()
-    {
-        if (_timer)
-        {
-            killTimer(_timer);
-        }
-    }
-
-    void CartesianImpedanceControllerWidgetController::setupGuiAfterConnect()
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        ARMARX_CHECK_NOT_NULL(_robot);
-        ARMARX_INFO << "setup comboBoxRNS";
-        for (const auto& rnsName : _robot->getRobotNodeSetNames())
-        {
-            _ui.comboBoxRNS->addItem(QString::fromStdString(rnsName));
-        }
-        ARMARX_TRACE;
-        for (const auto& nodeName : _robot->getRobotNodeNames())
-        {
-            _ui.comboBoxLockY->addItem(QString::fromStdString(nodeName));
-            _ui.comboBoxLockZ->addItem(QString::fromStdString(nodeName));
-        }
-
-        using T = CartesianImpedanceControllerWidgetController;
-        connect(_ui.pushButtonTargGet,                     &QPushButton::clicked, this, &T::on_pushButtonTargGet_clicked);
-        connect(_ui.pushButtonTargAdd,                     &QPushButton::clicked, this, &T::on_pushButtonTargAdd_clicked);
-        connect(_ui.pushButtonTargSet,                     &QPushButton::clicked, this, &T::on_pushButtonTargSet_clicked);
-        connect(_ui.comboBoxRNS,     SIGNAL(currentIndexChanged(const QString&)), this, SLOT(on_comboBoxRNS_currentIndexChanged(const QString&)));
-        connect(_ui.radioButtonSelectLeft,                &QRadioButton::clicked, this, &T::on_radioButtonSelect_clicked);
-        connect(_ui.radioButtonSelectRight,               &QRadioButton::clicked, this, &T::on_radioButtonSelect_clicked);
-        connect(_ui.radioButtonSelectRNS,                 &QRadioButton::clicked, this, &T::on_radioButtonSelect_clicked);
-
-        _ui.groupBoxAdvanced->setChecked(false);
-        _ui.radioButtonSelectLeft->setChecked(true);
-    }
-
-    void CartesianImpedanceControllerWidgetController::onConnectComponent()
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        _left  = getRobotNameHelper().getArm("Left");
-        _right = getRobotNameHelper().getArm("Right");
-    }
-
-    void CartesianImpedanceControllerWidgetController::timerEvent(QTimerEvent*)
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        if (_robot)
-        {
-            synchronizeLocalClone(_robot);
-        }
-        if (_ui.checkBoxAutoUpdatePose->isChecked())
-        {
-            ARMARX_TRACE;
-            on_pushButtonTargSet_clicked();
-        }
-        const bool lockY = _ui.checkBoxLockY->isChecked();
-        const bool lockZ = _ui.checkBoxLockZ->isChecked();
-        bool anyTracking = lockY || lockZ;
-        _ui.widgetTarget->setEnabled(!anyTracking);
-        if (_rns && (lockY || lockZ))
-        {
-            ARMARX_TRACE;
-            Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame();
-            if (lockY)
-            {
-                pose(1, 3) = _robot
-                             ->getRobotNode(_ui.comboBoxLockY->currentText().toStdString())
-                             ->getPositionInRootFrame()(1);
-            }
-            if (lockZ)
-            {
-                pose(2, 3) = _robot
-                             ->getRobotNode(_ui.comboBoxLockZ->currentText().toStdString())
-                             ->getPositionInRootFrame()(2);
-            }
-            if (_controller)
-            {
-                _controller->setPosition(pose.topRightCorner<3, 1>());
-            }
-            visuHandAtPose(pose);
-            ARMARX_INFO << "set Target pose to\n" << pose;
-        }
-        else if (!(lockY || lockZ))
-        {
-            const Eigen::Matrix4f rootTcpTarg =
-                _ui.radioButtonVisuSet ?
-                _targ.getMat4() :
-                _rns->getTCP()->getPoseInRootFrame() * _targ.getMat4();
-            visuHandAtPose(rootTcpTarg);
-        }
-    }
-
-    void CartesianImpedanceControllerWidgetController::visuHandAtPose(const Eigen::Matrix4f& tcpInRoot)
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        if (_visuHandRobotFile.empty() || !_robot || !arviz.topic())
-        {
-            if (arviz.topic())
-            {
-                arviz.commitLayerContaining("hand");
-            }
-            return;
-        }
-
-        arviz.commitLayerContaining(
-            "hand",
-            viz::Robot{"Hand"}
-            .file(_visuHandRobotProject, _visuHandRobotFile)
-            .useFullModel()
-            .pose(_robot->getGlobalPose() * tcpInRoot * _tcpToBase)
-            .overrideColor(viz::Color::green())
-        );
-    }
-}
-//read config
-namespace armarx
-{
-    NJointControllerConfigPtr CartesianImpedanceControllerWidgetController::readFullCFG() const
-    {
-        ARMARX_TRACE;
-        auto [pos, quat] = readTargetCFG();
-        return NJointControllerConfigPtr::dynamicCast(_settings->readFullCFG(pos, quat));
-    }
-
-    std::tuple<Eigen::Vector3f, Eigen::Quaternionf>
-    CartesianImpedanceControllerWidgetController::readTargetCFG() const
-    {
-        ARMARX_TRACE;
-        return {_targ.getPos(), _targ.getQuat()};
-    }
-}
-//push buttons
-namespace armarx
-{
-    void CartesianImpedanceControllerWidgetController::createController()
-    {
-        ARMARX_TRACE;
-        NJointControllerGuiPluginBase::createController();
-        _ui.widgetRNSSelect->setEnabled(false);
-        _settings->setController(_controller);
-    }
-
-    void CartesianImpedanceControllerWidgetController::deleteController()
-    {
-        ARMARX_TRACE;
-        NJointControllerGuiPluginBase::deleteController();
-        _ui.widgetRNSSelect->setEnabled(true);
-        _settings->setController(nullptr);
-    }
-
-    void CartesianImpedanceControllerWidgetController::on_pushButtonTargSet_clicked()
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        if (!_controller)
-        {
-            return;
-        }
-        const Eigen::Matrix4f pose = _targ.getMat4();
-        _controller->setPose(pose);
-        ARMARX_INFO << "set Target pose to\n" << pose;
-    }
-
-    void CartesianImpedanceControllerWidgetController::on_pushButtonTargAdd_clicked()
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        if (!_controller || !_rns)
-        {
-            return;
-        }
-        const Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame() * _targ.getMat4();
-        _controller->setPose(pose);
-        ARMARX_INFO << "set Target pose to\n" << pose;
-    }
-
-    void CartesianImpedanceControllerWidgetController::on_pushButtonTargGet_clicked()
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        if (!_rns)
-        {
-            return;
-        }
-        const Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame();
-        const Eigen::Vector3f pos = simox::math::mat4f_to_pos(pose);
-        const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(pose);
-
-        _ui.doubleSpinBoxTargTX->setValue(pos(0));
-        _ui.doubleSpinBoxTargTY->setValue(pos(1));
-        _ui.doubleSpinBoxTargTZ->setValue(pos(2));
-
-        _ui.doubleSpinBoxTargRX->setValue(rpy(0));
-        _ui.doubleSpinBoxTargRY->setValue(rpy(1));
-        _ui.doubleSpinBoxTargRZ->setValue(rpy(2));
-    }
-
-    void CartesianImpedanceControllerWidgetController::on_comboBoxRNS_currentIndexChanged(const QString& arg1)
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        if (!_robot || _controller)
-        {
-            return;
-        }
-        const std::string rnsName = arg1.toStdString();
-        _rns = _robot->getRobotNodeSet(rnsName);
-        _settings->setRNS(_rns);
-        auto checkSide = [&](const RobotNameHelper::Arm & side)
-        {
-            if (!_rns->getTCP() || side.getTCP() != _rns->getTCP()->getName() || !side.getRobotNameHelper())
-            {
-                return;
-            }
-            const auto arm = side.addRobot(_robot);
-            _tcpToBase = arm.getTcp2HandRootTransform();
-            _visuHandRobotFile = side.getHandModelPath();
-            _visuHandRobotProject = side.getHandModelPackage();
-        };
-        if (_left.getRobotNameHelper())
-        {
-            checkSide(_left);
-        }
-        if (_right.getRobotNameHelper())
-        {
-            checkSide(_right);
-        }
-
-        on_pushButtonTargGet_clicked();
-    }
-
-    void CartesianImpedanceControllerWidgetController::on_radioButtonSelect_clicked()
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        if (_ui.radioButtonSelectLeft->isChecked())
-        {
-            on_comboBoxRNS_currentIndexChanged(QString::fromStdString(_left.getKinematicChain()));
-        }
-        else if (_ui.radioButtonSelectRight->isChecked())
-        {
-            on_comboBoxRNS_currentIndexChanged(QString::fromStdString(_right.getKinematicChain()));
-        }
-        else
-        {
-            on_comboBoxRNS_currentIndexChanged(_ui.comboBoxRNS->currentText());
-        }
-    }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
deleted file mode 100644
index 1618ce6e6d047993ef1360d6ae7203cb79013296..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::gui-plugins::CartesianImpedanceControllerWidgetController
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2020
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToPose.h>
-#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
-#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-
-#include <RobotAPI/gui-plugins/CartesianImpedanceController/ui_CartesianImpedanceControllerWidget.h>
-
-namespace armarx
-{
-    /**
-    \page RobotAPI-GuiPlugins-CartesianImpedanceController CartesianImpedanceController
-    \brief The CartesianImpedanceController allows visualizing ...
-
-    \image html CartesianImpedanceController.png
-    The user can
-
-    API Documentation \ref CartesianImpedanceControllerWidgetController
-
-    \see CartesianImpedanceControllerGuiPlugin
-    */
-
-    /**
-     * \class CartesianImpedanceControllerWidgetController
-     * \brief CartesianImpedanceControllerWidgetController brief one line description
-     *
-     * Detailed description
-     */
-    class ARMARXCOMPONENT_IMPORT_EXPORT
-        CartesianImpedanceControllerWidgetController:
-        public NJointControllerGuiPluginBase <
-        CartesianImpedanceControllerWidgetController,
-        NJointTaskSpaceImpedanceControlInterfacePrx
-        >,
-        virtual public ArVizComponentPluginUser
-    {
-        Q_OBJECT
-
-    public:
-        explicit CartesianImpedanceControllerWidgetController();
-        ~CartesianImpedanceControllerWidgetController();
-
-        /**
-         * Returns the Widget name displayed in the ArmarXGui to create an
-         * instance of this class.
-         */
-        static QString GetWidgetName()
-        {
-            return "RobotControl.NJointControllers.CartesianImpedanceControl";
-        }
-        void setupGuiAfterConnect() override;
-        void onConnectComponent() override;
-
-    protected:
-        void timerEvent(QTimerEvent* event) override;
-        void visuHandAtPose(const Eigen::Matrix4f& tcpInRoot = Eigen::Matrix4f::Identity());
-
-    private slots:
-        void on_comboBoxRNS_currentIndexChanged(const QString& arg1);
-        void on_pushButtonTargGet_clicked();
-        void on_pushButtonTargAdd_clicked();
-        void on_pushButtonTargSet_clicked();
-        void on_radioButtonSelect_clicked();
-        void createController() override;
-        void deleteController() override;
-
-    private:
-        NJointControllerConfigPtr readFullCFG() const override;
-        std::tuple<Eigen::Vector3f, Eigen::Quaternionf> readTargetCFG() const;
-
-    private:
-        Ui::CartesianImpedanceControllerWidget  _ui;
-        VirtualRobot::RobotNodeSetPtr           _rns;
-        SpinBoxToPose<QDoubleSpinBox>           _targ;
-        CartesianImpedanceControllerConfigWidget* _settings;
-
-        RobotNameHelper::Arm _left;
-        RobotNameHelper::Arm _right;
-        std::string _visuHandRobotFile;
-        std::string _visuHandRobotProject;
-        Eigen::Matrix4f _tcpToBase;
-        int _timer{0};
-    };
-}
-
-
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CMakeLists.txt b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CMakeLists.txt
deleted file mode 100644
index 4eb90929e3e13b501967b7d6fceaf3d0e62d81f4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CMakeLists.txt
+++ /dev/null
@@ -1,31 +0,0 @@
-armarx_set_target("CartesianNaturalPositionControllerGuiPlugin")
-
-armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
-
-set(SOURCES
-    CartesianNaturalPositionControllerGuiPlugin.cpp
-    CartesianNaturalPositionControllerWidgetController.cpp
-)
-
-set(HEADERS
-    CartesianNaturalPositionControllerGuiPlugin.h
-    CartesianNaturalPositionControllerWidgetController.h
-)
-
-set(GUI_MOC_HDRS ${HEADERS})
-
-set(GUI_UIS CartesianNaturalPositionControllerWidget.ui)
-
-set(COMPONENT_LIBS
-    VirtualRobot
-
-    SimpleConfigDialog
-
-    RobotAPIInterfaces
-    RobotAPICore
-    natik
-)
-
-if(ArmarXGui_FOUND)
-    armarx_gui_library(CartesianNaturalPositionControllerGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.cpp
deleted file mode 100644
index 022df8f02187da3f358f7e61e8a4289f1c8e1bb7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::CartesianNaturalPositionControllerGuiPlugin
- * \author     armar-user ( armar-user at kit dot edu )
- * \date       2020
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "CartesianNaturalPositionControllerGuiPlugin.h"
-
-#include "CartesianNaturalPositionControllerWidgetController.h"
-
-namespace armarx
-{
-    CartesianNaturalPositionControllerGuiPlugin::CartesianNaturalPositionControllerGuiPlugin()
-    {
-        addWidget < CartesianNaturalPositionControllerWidgetController > ();
-    }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.h b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.h
deleted file mode 100644
index 5007d8320b4841dd6186912f24eca031f5a03c46..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::CartesianNaturalPositionController
- * \author     armar-user ( armar-user at kit dot edu )
- * \date       2020
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-
-namespace armarx
-{
-    /**
-     * \class CartesianNaturalPositionControllerGuiPlugin
-     * \ingroup ArmarXGuiPlugins
-     * \brief CartesianNaturalPositionControllerGuiPlugin brief description
-     *
-     * Detailed description
-     */
-    class ARMARXCOMPONENT_IMPORT_EXPORT CartesianNaturalPositionControllerGuiPlugin:
-        public armarx::ArmarXGuiPlugin
-    {
-        Q_OBJECT
-        Q_INTERFACES(ArmarXGuiInterface)
-        Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
-    public:
-        /**
-         * All widgets exposed by this plugin are added in the constructor
-         * via calls to addWidget()
-         */
-        CartesianNaturalPositionControllerGuiPlugin();
-    };
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
deleted file mode 100644
index 998d97de62e1a976636ce4ca531cb03f396126f7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
+++ /dev/null
@@ -1,430 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>CartesianNaturalPositionControllerWidget</class>
- <widget class="QWidget" name="CartesianNaturalPositionControllerWidget">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>767</width>
-    <height>493</height>
-   </rect>
-  </property>
-  <property name="windowTitle">
-   <string>CartesianNaturalPositionControllerWidget</string>
-  </property>
-  <widget class="QWidget" name="widget" native="true">
-   <property name="geometry">
-    <rect>
-     <x>20</x>
-     <y>20</y>
-     <width>191</width>
-     <height>101</height>
-    </rect>
-   </property>
-   <layout class="QGridLayout" name="gridLayout">
-    <item row="0" column="0">
-     <widget class="QLabel" name="label">
-      <property name="text">
-       <string>Side</string>
-      </property>
-     </widget>
-    </item>
-    <item row="0" column="1">
-     <widget class="QComboBox" name="comboBoxSide"/>
-    </item>
-    <item row="1" column="0">
-     <widget class="QPushButton" name="pushButtonCreateController">
-      <property name="text">
-       <string>Create</string>
-      </property>
-     </widget>
-    </item>
-   </layout>
-  </widget>
-  <widget class="QGroupBox" name="groupBox_2">
-   <property name="geometry">
-    <rect>
-     <x>20</x>
-     <y>140</y>
-     <width>321</width>
-     <height>181</height>
-    </rect>
-   </property>
-   <property name="title">
-    <string>Control target</string>
-   </property>
-   <layout class="QGridLayout" name="gridLayout_2">
-    <item row="2" column="3">
-     <widget class="QPushButton" name="pushButtonRxp">
-      <property name="text">
-       <string>rx+</string>
-      </property>
-     </widget>
-    </item>
-    <item row="2" column="5">
-     <widget class="QPushButton" name="pushButtonRzp">
-      <property name="text">
-       <string>rz+</string>
-      </property>
-     </widget>
-    </item>
-    <item row="2" column="4">
-     <widget class="QPushButton" name="pushButtonRyp">
-      <property name="text">
-       <string>ry+</string>
-      </property>
-     </widget>
-    </item>
-    <item row="1" column="3" colspan="3">
-     <widget class="QWidget" name="widget_3" native="true">
-      <layout class="QGridLayout" name="gridLayout_4">
-       <property name="leftMargin">
-        <number>0</number>
-       </property>
-       <property name="topMargin">
-        <number>0</number>
-       </property>
-       <property name="rightMargin">
-        <number>0</number>
-       </property>
-       <property name="bottomMargin">
-        <number>0</number>
-       </property>
-       <property name="verticalSpacing">
-        <number>0</number>
-       </property>
-       <item row="0" column="0">
-        <widget class="QLabel" name="label_3">
-         <property name="text">
-          <string>dOri</string>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="1">
-        <widget class="QSpinBox" name="spinBoxDori">
-         <property name="value">
-          <number>10</number>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="2">
-        <spacer name="horizontalSpacer_2">
-         <property name="orientation">
-          <enum>Qt::Horizontal</enum>
-         </property>
-         <property name="sizeHint" stdset="0">
-          <size>
-           <width>40</width>
-           <height>20</height>
-          </size>
-         </property>
-        </spacer>
-       </item>
-      </layout>
-     </widget>
-    </item>
-    <item row="3" column="3">
-     <widget class="QPushButton" name="pushButtonRxn">
-      <property name="text">
-       <string>rx-</string>
-      </property>
-     </widget>
-    </item>
-    <item row="1" column="0" colspan="3">
-     <widget class="QWidget" name="widget_2" native="true">
-      <layout class="QGridLayout" name="gridLayout_3">
-       <property name="leftMargin">
-        <number>0</number>
-       </property>
-       <property name="topMargin">
-        <number>0</number>
-       </property>
-       <property name="rightMargin">
-        <number>0</number>
-       </property>
-       <property name="bottomMargin">
-        <number>0</number>
-       </property>
-       <property name="verticalSpacing">
-        <number>0</number>
-       </property>
-       <item row="0" column="1">
-        <widget class="QSpinBox" name="spinBoxDpos">
-         <property name="maximum">
-          <number>500</number>
-         </property>
-         <property name="value">
-          <number>100</number>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="0">
-        <widget class="QLabel" name="label_2">
-         <property name="text">
-          <string>dPos</string>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="2">
-        <spacer name="horizontalSpacer">
-         <property name="orientation">
-          <enum>Qt::Horizontal</enum>
-         </property>
-         <property name="sizeHint" stdset="0">
-          <size>
-           <width>40</width>
-           <height>20</height>
-          </size>
-         </property>
-        </spacer>
-       </item>
-      </layout>
-     </widget>
-    </item>
-    <item row="0" column="3" colspan="3">
-     <widget class="QCheckBox" name="checkBoxSetOri">
-      <property name="text">
-       <string>Set Orientation</string>
-      </property>
-      <property name="checked">
-       <bool>true</bool>
-      </property>
-     </widget>
-    </item>
-    <item row="3" column="0">
-     <widget class="QPushButton" name="pushButtonPxn">
-      <property name="text">
-       <string>px-</string>
-      </property>
-     </widget>
-    </item>
-    <item row="3" column="1">
-     <widget class="QPushButton" name="pushButtonPyn">
-      <property name="text">
-       <string>py-</string>
-      </property>
-     </widget>
-    </item>
-    <item row="3" column="5">
-     <widget class="QPushButton" name="pushButtonRzn">
-      <property name="text">
-       <string>rz-</string>
-      </property>
-     </widget>
-    </item>
-    <item row="3" column="2">
-     <widget class="QPushButton" name="pushButtonPzn">
-      <property name="text">
-       <string>pz-</string>
-      </property>
-     </widget>
-    </item>
-    <item row="2" column="1">
-     <widget class="QPushButton" name="pushButtonPyp">
-      <property name="text">
-       <string>py+</string>
-      </property>
-     </widget>
-    </item>
-    <item row="3" column="4">
-     <widget class="QPushButton" name="pushButtonRyn">
-      <property name="text">
-       <string>ry-</string>
-      </property>
-     </widget>
-    </item>
-    <item row="2" column="0">
-     <widget class="QPushButton" name="pushButtonPxp">
-      <property name="text">
-       <string>px+</string>
-      </property>
-     </widget>
-    </item>
-    <item row="2" column="2">
-     <widget class="QPushButton" name="pushButtonPzp">
-      <property name="text">
-       <string>pz+</string>
-      </property>
-     </widget>
-    </item>
-    <item row="4" column="0">
-     <spacer name="verticalSpacer">
-      <property name="orientation">
-       <enum>Qt::Vertical</enum>
-      </property>
-      <property name="sizeHint" stdset="0">
-       <size>
-        <width>20</width>
-        <height>40</height>
-       </size>
-      </property>
-     </spacer>
-    </item>
-   </layout>
-  </widget>
-  <widget class="QGroupBox" name="groupBox">
-   <property name="geometry">
-    <rect>
-     <x>20</x>
-     <y>330</y>
-     <width>321</width>
-     <height>121</height>
-    </rect>
-   </property>
-   <property name="title">
-    <string>Parameters</string>
-   </property>
-   <layout class="QGridLayout" name="gridLayout_5">
-    <item row="2" column="0">
-     <widget class="QLabel" name="label_4">
-      <property name="text">
-       <string>KpElbow</string>
-      </property>
-     </widget>
-    </item>
-    <item row="3" column="1">
-     <widget class="QSlider" name="sliderKpJla">
-      <property name="maximum">
-       <number>200</number>
-      </property>
-      <property name="orientation">
-       <enum>Qt::Horizontal</enum>
-      </property>
-     </widget>
-    </item>
-    <item row="3" column="0">
-     <widget class="QLabel" name="label_5">
-      <property name="text">
-       <string>KpJLA</string>
-      </property>
-     </widget>
-    </item>
-    <item row="3" column="2">
-     <widget class="QLabel" name="labelKpJla">
-      <property name="text">
-       <string>0.0</string>
-      </property>
-     </widget>
-    </item>
-    <item row="2" column="1">
-     <widget class="QSlider" name="sliderKpElbow">
-      <property name="maximum">
-       <number>200</number>
-      </property>
-      <property name="singleStep">
-       <number>1</number>
-      </property>
-      <property name="value">
-       <number>100</number>
-      </property>
-      <property name="orientation">
-       <enum>Qt::Horizontal</enum>
-      </property>
-     </widget>
-    </item>
-    <item row="2" column="2">
-     <widget class="QLabel" name="labelKpElbow">
-      <property name="text">
-       <string>0.0</string>
-      </property>
-     </widget>
-    </item>
-    <item row="1" column="0" colspan="2">
-     <widget class="QCheckBox" name="checkBoxAutoKp">
-      <property name="text">
-       <string>Automatic Kp</string>
-      </property>
-     </widget>
-    </item>
-    <item row="0" column="0">
-     <widget class="QLabel" name="label_6">
-      <property name="text">
-       <string>PosVel</string>
-      </property>
-     </widget>
-    </item>
-    <item row="0" column="1">
-     <widget class="QSlider" name="sliderPosVel">
-      <property name="maximum">
-       <number>400</number>
-      </property>
-      <property name="value">
-       <number>80</number>
-      </property>
-      <property name="orientation">
-       <enum>Qt::Horizontal</enum>
-      </property>
-     </widget>
-    </item>
-    <item row="0" column="2">
-     <widget class="QLabel" name="labelPosVel">
-      <property name="text">
-       <string>80</string>
-      </property>
-     </widget>
-    </item>
-   </layout>
-  </widget>
-  <widget class="QGroupBox" name="groupBoxNullspaceTargets">
-   <property name="geometry">
-    <rect>
-     <x>390</x>
-     <y>160</y>
-     <width>371</width>
-     <height>291</height>
-    </rect>
-   </property>
-   <property name="title">
-    <string>Nullspace Targets</string>
-   </property>
-   <layout class="QGridLayout" name="gridLayout_6">
-    <item row="1" column="1">
-     <widget class="QWidget" name="widgetNullspaceTargets" native="true">
-      <layout class="QGridLayout" name="gridLayoutNullspaceTargets">
-       <item row="0" column="1">
-        <widget class="QSlider" name="horizontalSliderExampleJoint1">
-         <property name="maximum">
-          <number>360</number>
-         </property>
-         <property name="orientation">
-          <enum>Qt::Horizontal</enum>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="0">
-        <widget class="QCheckBox" name="checkBoxExampleJoint1">
-         <property name="text">
-          <string>Joint1</string>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="2">
-        <widget class="QLabel" name="labelExampleJoint1">
-         <property name="text">
-          <string>0</string>
-         </property>
-        </widget>
-       </item>
-      </layout>
-     </widget>
-    </item>
-    <item row="2" column="1">
-     <spacer name="verticalSpacer_2">
-      <property name="orientation">
-       <enum>Qt::Vertical</enum>
-      </property>
-      <property name="sizeHint" stdset="0">
-       <size>
-        <width>20</width>
-        <height>40</height>
-       </size>
-      </property>
-     </spacer>
-    </item>
-   </layout>
-  </widget>
- </widget>
- <resources/>
- <connections/>
-</ui>
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
deleted file mode 100644
index f14d3a669f9433a4400b3a7fdcf5193708404c07..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
+++ /dev/null
@@ -1,399 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::CartesianNaturalPositionControllerWidgetController
- * \author     armar-user ( armar-user at kit dot edu )
- * \date       2020
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "CartesianNaturalPositionControllerWidgetController.h"
-
-#include <VirtualRobot/math/Helpers.h>
-
-#include <string>
-
-namespace armarx
-{
-    CartesianNaturalPositionControllerWidgetController::CartesianNaturalPositionControllerWidgetController()
-    {
-        std::lock_guard g{_allMutex};
-        _ui.setupUi(getWidget());
-
-        connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection);
-        connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection);
-
-
-        //connect(_ui.pushButtonStop,             SIGNAL(clicked()), this, SLOT(on_pushButtonStop_clicked()));
-        //connect(_ui.pushButtonExecute,          SIGNAL(clicked()), this, SLOT(on_pushButtonExecute_clicked()));
-        //connect(_ui.pushButtonZeroFT,           SIGNAL(clicked()), this, SLOT(on_pushButtonZeroFT_clicked()));
-        //connect(_ui.pushButtonSendSettings,     SIGNAL(clicked()), this, SLOT(on_pushButtonSendSettings_clicked()));
-        connect(_ui.pushButtonCreateController, SIGNAL(clicked()), this, SLOT(on_pushButtonCreateController_clicked()));
-        connect(_ui.sliderKpElbow, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
-        connect(_ui.sliderKpJla, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
-        connect(_ui.checkBoxAutoKp, SIGNAL(stateChanged(int)), this, SLOT(on_checkBoxAutoKp_stateChanged(int)));
-        connect(_ui.checkBoxSetOri, SIGNAL(stateChanged(int)), this, SLOT(on_checkBoxSetOri_stateChanged(int)));
-        connect(_ui.sliderPosVel, SIGNAL(valueChanged(int)), this, SLOT(on_horizontalSliderPosVel_valueChanged(int)));
-
-        auto addBtn = [&](QPushButton * btn, float px, float py, float pz, float rx, float ry, float rz)
-        {
-            _deltaMapPos[btn] = {px, py, pz};
-            _deltaMapOri[btn] = {rx, ry, rz};
-            //ARMARX_IMPORTANT << "connect";
-            //ARMARX_IMPORTANT << btn->text().toStdString();
-            connect(btn, SIGNAL(clicked()), this, SLOT(on_anyDeltaPushButton_clicked()));
-        };
-        addBtn(_ui.pushButtonPxp, +1, 0, 0, 0, 0, 0);
-        addBtn(_ui.pushButtonPxn, -1, 0, 0, 0, 0, 0);
-        addBtn(_ui.pushButtonPyp, 0, +1, 0, 0, 0, 0);
-        addBtn(_ui.pushButtonPyn, 0, -1, 0, 0, 0, 0);
-        addBtn(_ui.pushButtonPzp, 0, 0, +1, 0, 0, 0);
-        addBtn(_ui.pushButtonPzn, 0, 0, -1, 0, 0, 0);
-
-        addBtn(_ui.pushButtonRxp, 0, 0, 0, +1, 0, 0);
-        addBtn(_ui.pushButtonRxn, 0, 0, 0, -1, 0, 0);
-        addBtn(_ui.pushButtonRyp, 0, 0, 0, 0, +1, 0);
-        addBtn(_ui.pushButtonRyn, 0, 0, 0, 0, -1, 0);
-        addBtn(_ui.pushButtonRzp, 0, 0, 0, 0, 0, +1);
-        addBtn(_ui.pushButtonRzn, 0, 0, 0, 0, 0, -1);
-
-        //_ui.widgetSpacer->setVisible(false);
-        _timer = startTimer(100);
-    }
-
-
-    CartesianNaturalPositionControllerWidgetController::~CartesianNaturalPositionControllerWidgetController()
-    {
-        killTimer(_timer);
-    }
-
-
-    void CartesianNaturalPositionControllerWidgetController::loadSettings(QSettings* settings)
-    {
-        std::lock_guard g{_allMutex};
-        _robotStateComponentName = settings->value("rsc", "Armar6StateComponent").toString().toStdString();
-        _robotUnitName = settings->value("ru", "Armar6Unit").toString().toStdString();
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::saveSettings(QSettings* settings)
-    {
-        std::lock_guard g{_allMutex};
-        settings->setValue("rsc", QString::fromStdString(_robotStateComponentName));
-        settings->setValue("ru", QString::fromStdString(_robotUnitName));
-    }
-
-
-    void CartesianNaturalPositionControllerWidgetController::onInitComponent()
-    {
-        std::lock_guard g{_allMutex};
-        usingProxy(_robotStateComponentName);
-        usingProxy(_robotUnitName);
-    }
-
-
-    void CartesianNaturalPositionControllerWidgetController::onConnectComponent()
-    {
-        std::lock_guard g{_allMutex};
-        //proxies
-        {
-            _robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(_robotStateComponentName);
-            _robotUnit = getProxy<RobotUnitInterfacePrx>(_robotUnitName);
-        }
-        //robot
-        {
-            _robot = RemoteRobot::createLocalCloneFromFile(_robotStateComponent, VirtualRobot::RobotIO::eStructure);
-        }
-
-        _controller.reset();
-        invokeConnectComponentQt();
-    }
-    void CartesianNaturalPositionControllerWidgetController::onConnectComponentQt()
-    {
-        _ui.comboBoxSide->addItem("Right");
-        _ui.comboBoxSide->addItem("Left");
-        _ui.comboBoxSide->setCurrentIndex(0);
-
-    }
-    void CartesianNaturalPositionControllerWidgetController::onDisconnectComponent()
-    {
-        std::lock_guard g{_allMutex};
-        deleteOldController();
-        _robotStateComponent = nullptr;
-    }
-    void CartesianNaturalPositionControllerWidgetController::on_pushButtonCreateController_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        deleteOldController();
-
-        std::string side = _ui.comboBoxSide->currentText().toStdString();
-
-        VirtualRobot::RobotNodeSetPtr rns = _robot->getRobotNodeSet(side + "Arm");
-
-        _ui.gridLayoutNullspaceTargets->removeWidget(_ui.checkBoxExampleJoint1);
-        _ui.gridLayoutNullspaceTargets->removeWidget(_ui.labelExampleJoint1);
-        _ui.gridLayoutNullspaceTargets->removeWidget(_ui.horizontalSliderExampleJoint1);
-
-        for (size_t i = 0; i < rns->getSize(); i++)
-        {
-            QCheckBox* checkBox = new QCheckBox(QString::fromStdString(rns->getNode(i)->getName()));
-            QSlider* slider = new QSlider(Qt::Orientation::Horizontal);
-            slider->setMinimum(rns->getNode(i)->getJointLimitLo() / M_PI * 180);
-            slider->setMaximum(rns->getNode(i)->getJointLimitHi() / M_PI * 180);
-            slider->setValue(rns->getNode(i)->getJointValue() / M_PI * 180);
-            QLabel* label = new QLabel(QString::number(slider->value()));
-            _ui.gridLayoutNullspaceTargets->addWidget(checkBox, i, 0);
-            _ui.gridLayoutNullspaceTargets->addWidget(slider, i, 1);
-            _ui.gridLayoutNullspaceTargets->addWidget(label, i, 2);
-            connect(checkBox, SIGNAL(stateChanged(int)), this, SLOT(on_anyNullspaceCheckbox_stateChanged(int)));
-            connect(slider, SIGNAL(valueChanged(int)), this, SLOT(on_anyNullspaceSlider_valueChanged(int)));
-            NullspaceTarget nt;
-            nt.checkBox = checkBox;
-            nt.slider = slider;
-            nt.label = label;
-            nt.i = i;
-            _nullspaceTargets.emplace_back(nt);
-        }
-
-
-        VirtualRobot::RobotNodePtr cla = rns->getNode(0);
-        VirtualRobot::RobotNodePtr sho1 = rns->getNode(1);
-        ARMARX_IMPORTANT << VAROUT(cla->getJointValue());
-        cla->setJointValue(0);
-        Eigen::Vector3f shoulder = sho1->getPositionInRootFrame();
-        VirtualRobot::RobotNodePtr elb = rns->getNode(4);
-        VirtualRobot::RobotNodePtr wri1 = rns->getNode(6);
-        VirtualRobot::RobotNodePtr tcp = rns->getTCP();
-        NaturalIK::ArmJoints arm;
-
-        arm.rns = rns;
-        arm.shoulder = sho1;
-        arm.elbow = elb;
-        arm.tcp = tcp;
-
-        std::vector<float> jointCosts = std::vector<float>(rns->getSize(), 1);
-        jointCosts.at(0) = 4;
-
-
-        armarx::RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
-        _tcpTarget = rns->getTCP()->getPoseInRootFrame();
-
-        NaturalIK ik(side, shoulder, 1.3f);
-        float upper_arm_length = (sho1->getPositionInRootFrame() - elb->getPositionInRootFrame()).norm();
-        float lower_arm_length = (elb->getPositionInRootFrame() - wri1->getPositionInRootFrame()).norm()
-                                 + (wri1->getPositionInRootFrame() - tcp->getPositionInRootFrame()).norm();
-        ik.setUpperArmLength(upper_arm_length);
-        ik.setLowerArmLength(lower_arm_length);
-        NJointCartesianNaturalPositionControllerConfigPtr config = CartesianNaturalPositionControllerProxy::MakeConfig(rns->getName(), elb->getName());
-        config->runCfg.jointCosts = jointCosts;
-        CartesianNaturalPositionControllerConfig runCfg = config->runCfg;
-        updateKpSliders(runCfg);
-        //config->runCfg = runCfg;
-        _controller.reset(new CartesianNaturalPositionControllerProxy(ik, arm, _robotUnit, side + "NaturalPosition", config));
-        _controller->init();
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::on_anyDeltaPushButton_clicked()
-    {
-        Eigen::Matrix4f newTarget = _tcpTarget;
-        ARMARX_IMPORTANT << "on_anyDeltaPushButton_clicked";
-        std::lock_guard g{_allMutex};
-        Eigen::Vector3f deltaPos = _deltaMapPos.at(QObject::sender());
-        Eigen::Vector3f deltaOri = _deltaMapOri.at(QObject::sender());
-        newTarget = math::Helpers::TranslatePose(newTarget, deltaPos * _ui.spinBoxDpos->value());
-        if (deltaOri.norm() > 0)
-        {
-            deltaOri = deltaOri * _ui.spinBoxDori->value() / 180 * M_PI;
-            Eigen::AngleAxisf aa(deltaOri.norm(), deltaOri.normalized());
-            math::Helpers::Orientation(newTarget) = aa.toRotationMatrix() * math::Helpers::Orientation(newTarget);
-        }
-        updateTarget(newTarget);
-    }
-    void CartesianNaturalPositionControllerWidgetController::updateTarget(const Eigen::Matrix4f& newTarget)
-    {
-        if (!_controller->setTarget(newTarget, _setOri ? NaturalDiffIK::Mode::All : NaturalDiffIK::Mode::Position))
-        {
-            return;
-        }
-        _tcpTarget = newTarget;
-        if (_controller->getDynamicKp().enabled)
-        {
-            updateKpSliders(_controller->getRuntimeConfig());
-        }
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::readRunCfgFromUi(CartesianNaturalPositionControllerConfig& runCfg)
-    {
-        runCfg.elbowKp = _ui.sliderKpElbow->value() * 0.01f;
-        runCfg.jointLimitAvoidanceKp = _ui.sliderKpJla->value() * 0.01f;
-    }
-    void CartesianNaturalPositionControllerWidgetController::updateKpSliderLabels(const CartesianNaturalPositionControllerConfig& runCfg)
-    {
-        _ui.labelKpElbow->setText(QString::number(runCfg.elbowKp, 'f', 2));
-        _ui.labelKpJla->setText(QString::number(runCfg.jointLimitAvoidanceKp, 'f', 2));
-    }
-    void CartesianNaturalPositionControllerWidgetController::updateKpSliders(const CartesianNaturalPositionControllerConfig& runCfg)
-    {
-        const QSignalBlocker blockKpElb(_ui.sliderKpElbow);
-        const QSignalBlocker blockKpJla(_ui.sliderKpJla);
-        _ui.sliderKpElbow->setValue(runCfg.elbowKp * 100);
-        _ui.sliderKpJla->setValue(runCfg.jointLimitAvoidanceKp * 100);
-        updateKpSliderLabels(runCfg);
-        ARMARX_IMPORTANT << VAROUT(runCfg.elbowKp) << VAROUT(runCfg.jointLimitAvoidanceKp);
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::on_sliders_valueChanged(int)
-    {
-        CartesianNaturalPositionControllerConfig runCfg = _controller->getRuntimeConfig();
-        //ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
-        readRunCfgFromUi(runCfg);
-        updateKpSliderLabels(runCfg);
-        _controller->setRuntimeConfig(runCfg);
-        _controller->updateBaseKpValues(runCfg);
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::on_checkBoxAutoKp_stateChanged(int)
-    {
-        CartesianNaturalPositionControllerProxy::DynamicKp dynamicKp;
-        dynamicKp.enabled = _ui.checkBoxAutoKp->isChecked();
-        _controller->setDynamicKp(dynamicKp);
-        if (_controller->getDynamicKp().enabled)
-        {
-            updateKpSliders(_controller->getRuntimeConfig());
-        }
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::on_checkBoxSetOri_stateChanged(int)
-    {
-        _setOri = _ui.checkBoxSetOri->isChecked();
-        updateTarget(_tcpTarget);
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::updateNullspaceTargets()
-    {
-        std::vector<float> nsTargets;
-        for (const NullspaceTarget& nt : _nullspaceTargets)
-        {
-            nsTargets.push_back(nt.checkBox->isChecked() ? nt.slider->value() * M_PI / 180 : std::nanf(""));
-            nt.label->setText(QString::number(nt.slider->value()));
-        }
-        _controller->setNullspaceTarget(nsTargets);
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::on_anyNullspaceCheckbox_stateChanged(int)
-    {
-        updateNullspaceTargets();
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::on_anyNullspaceSlider_valueChanged(int)
-    {
-        updateNullspaceTargets();
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::on_horizontalSliderPosVel_valueChanged(int)
-    {
-        _ui.labelPosVel->setText(QString::number(_ui.sliderPosVel->value()));
-        float posVel = _ui.sliderPosVel->value();
-        _controller->setMaxVelocities(posVel);
-        //_runCfg = _controller->getRuntimeConfig();
-    }
-
-    //void CartesianNaturalPositionControllerWidgetController::on_pushButtonSendSettings_clicked()
-    //{
-    //    std::lock_guard g{_allMutex};
-    //    if (_controller)
-    //    {
-    //        ARMARX_IMPORTANT << "sending new config to " << _controllerName;
-    //        _controller->setConfig(readRunCfg());
-    //    }
-    //}
-
-
-    QPointer<QDialog> CartesianNaturalPositionControllerWidgetController::getConfigDialog(QWidget* parent)
-    {
-        std::lock_guard g{_allMutex};
-        if (!_dialog)
-        {
-            _dialog = new SimpleConfigDialog(parent);
-            _dialog->addProxyFinder<RobotUnitInterfacePrx>({"ru", "Robot Unit", "*Unit"});
-            _dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"rsc", "Robot State Component", "*Component"});
-        }
-        return qobject_cast<SimpleConfigDialog*>(_dialog);
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::configured()
-    {
-        std::lock_guard g{_allMutex};
-        _robotStateComponentName = _dialog->getProxyName("rsc");
-        _robotUnitName = _dialog->getProxyName("ru");
-    }
-
-    /*NJointCartesianWaypointControllerRuntimeConfig CartesianWaypointControlGuiWidgetController::readRunCfg() const
-    {
-        std::lock_guard g{_allMutex};
-        NJointCartesianWaypointControllerRuntimeConfig cfg;
-
-        cfg.wpCfg.maxPositionAcceleration     = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
-        cfg.wpCfg.maxOrientationAcceleration  = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
-        cfg.wpCfg.maxNullspaceAcceleration    = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
-
-        cfg.wpCfg.kpJointLimitAvoidance       = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
-        cfg.wpCfg.jointLimitAvoidanceScale    = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
-
-        cfg.wpCfg.thresholdOrientationNear    = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
-        cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
-        cfg.wpCfg.thresholdPositionNear       = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
-        cfg.wpCfg.thresholdPositionReached    = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
-
-        cfg.wpCfg.maxOriVel                   = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
-        cfg.wpCfg.maxPosVel                   = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
-        cfg.wpCfg.kpOri                       = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
-        cfg.wpCfg.kpPos                       = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
-
-        cfg.forceThreshold                    = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
-        cfg.optimizeNullspaceIfTargetWasReached = _ui.checkBoxKeepOptimizing->isChecked();
-        cfg.forceThresholdInRobotRootZ          = _ui.checkBoxLimitinZ->isChecked();
-
-        return cfg;
-    }*/
-
-    void CartesianNaturalPositionControllerWidgetController::deleteOldController()
-    {
-        std::lock_guard g{_allMutex};
-        if (_controller)
-        {
-            _controller->cleanup();
-        }
-    }
-
-    void CartesianNaturalPositionControllerWidgetController::timerEvent(QTimerEvent*)
-    {
-        std::lock_guard g{_allMutex};
-        if (!_robot || !_robotStateComponent)
-        {
-            return;
-        }
-        RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
-        if (_controller)
-        {
-            ARMARX_INFO << deactivateSpam() << "setting visu gp to:\n" << _robot->getGlobalPose();
-            _controller->getInternalController()->setVisualizationRobotGlobalPose(_robot->getGlobalPose());
-        }
-    }
-
-
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
deleted file mode 100644
index 383bfd544aff904db1afe911363613dfa2dcf269..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
+++ /dev/null
@@ -1,163 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::gui-plugins::CartesianNaturalPositionControllerWidgetController
- * @author     armar-user ( armar-user at kit dot edu )
- * @date       2020
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <mutex>
-
-#include <VirtualRobot/Robot.h>
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-
-#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-
-#include <RobotAPI/gui-plugins/CartesianNaturalPositionController/ui_CartesianNaturalPositionControllerWidget.h>
-#include <RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h>
-
-namespace armarx
-{
-    /**
-    \page RobotAPI-GuiPlugins-CartesianNaturalPositionController CartesianNaturalPositionController
-    \brief The CartesianNaturalPositionController allows visualizing ...
-
-    \image html CartesianNaturalPositionController.png
-    The user can
-
-    API Documentation \ref CartesianNaturalPositionControllerWidgetController
-
-    \see CartesianNaturalPositionControllerGuiPlugin
-    */
-
-    /**
-     * \class CartesianNaturalPositionControllerWidgetController
-     * \brief CartesianNaturalPositionControllerWidgetController brief one line description
-     *
-     * Detailed description
-     */
-    class ARMARXCOMPONENT_IMPORT_EXPORT
-        CartesianNaturalPositionControllerWidgetController:
-        public armarx::ArmarXComponentWidgetControllerTemplate < CartesianNaturalPositionControllerWidgetController >
-    {
-        Q_OBJECT
-
-    public:
-        struct NullspaceTarget
-        {
-            QCheckBox* checkBox;
-            QSlider* slider;
-            QLabel* label;
-            size_t i;
-        };
-
-        /**
-         * Controller Constructor
-         */
-        explicit CartesianNaturalPositionControllerWidgetController();
-
-        /**
-         * Controller destructor
-         */
-        virtual ~CartesianNaturalPositionControllerWidgetController();
-
-        /**
-         * @see ArmarXWidgetController::loadSettings()
-         */
-        void loadSettings(QSettings* settings) override;
-
-        /**
-         * @see ArmarXWidgetController::saveSettings()
-         */
-        void saveSettings(QSettings* settings) override;
-
-        /**
-         * Returns the Widget name displayed in the ArmarXGui to create an
-         * instance of this class.
-         */
-        static QString GetWidgetName()
-        {
-            return "RobotControl.NJointControllers.CartesianNaturalPositionController";
-        }
-
-        void onInitComponent() override;
-        void onConnectComponent() override;
-        void onDisconnectComponent() override;
-
-        QPointer<QDialog> getConfigDialog(QWidget* parent) override;
-        void configured() override;
-
-    public slots:
-        /* QT slot declarations */
-        void onConnectComponentQt();
-        void on_pushButtonCreateController_clicked();
-        void on_anyDeltaPushButton_clicked();
-        void on_sliders_valueChanged(int);
-        void on_checkBoxAutoKp_stateChanged(int);
-        void on_checkBoxSetOri_stateChanged(int);
-        void on_anyNullspaceCheckbox_stateChanged(int);
-        void on_anyNullspaceSlider_valueChanged(int);
-        void on_horizontalSliderPosVel_valueChanged(int);
-
-    signals:
-        /* QT signal declarations */
-        void invokeConnectComponentQt();
-        void invokeDisconnectComponentQt();
-
-    private:
-        void deleteOldController();
-        void readRunCfgFromUi(CartesianNaturalPositionControllerConfig& runCfg);
-        void timerEvent(QTimerEvent*) override;
-        void updateTarget(const Eigen::Matrix4f& newTarget);
-        void updateKpSliders(const CartesianNaturalPositionControllerConfig& runCfg);
-        void updateKpSliderLabels(const CartesianNaturalPositionControllerConfig& runCfg);
-        void updateNullspaceTargets();
-
-
-
-        std::string                                     _robotStateComponentName;
-        std::string                                     _robotUnitName;
-        RobotStateComponentInterfacePrx                 _robotStateComponent;
-        RobotUnitInterfacePrx                           _robotUnit;
-        Ui::CartesianNaturalPositionControllerWidget    _ui;
-        QPointer<SimpleConfigDialog>                    _dialog;
-        CartesianNaturalPositionControllerProxyPtr      _controller;
-        std::string                                     _controllerName;
-        VirtualRobot::RobotPtr                          _robot;
-        std::vector<Eigen::Matrix4f>                    _lastParsedWPs;
-        bool                                            _supportsFT{false};
-        bool                                            _setOri = true;
-        mutable std::recursive_mutex                    _allMutex;
-        int                                             _timer;
-        Eigen::Matrix4f                                 _tcpTarget;
-
-        std::map<QObject*, Eigen::Vector3f>             _deltaMapPos;
-        std::map<QObject*, Eigen::Vector3f>             _deltaMapOri;
-        //CartesianNaturalPositionControllerConfig        _runCfg;
-
-        std::vector<NullspaceTarget>                    _nullspaceTargets;
-    };
-}
-
-
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
deleted file mode 100644
index 55b06608d60a5a3f09f6805a20039177f0bb8fc3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
+++ /dev/null
@@ -1,18 +0,0 @@
-armarx_set_target("CartesianWaypointControlGuiGuiPlugin")
-armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
-
-set(SOURCES
-    CartesianWaypointControlGuiGuiPlugin.cpp
-    CartesianWaypointControlGuiWidgetController.cpp
-)
-set(HEADERS
-    CartesianWaypointControlGuiGuiPlugin.h
-    CartesianWaypointControlGuiWidgetController.h
-)
-set(GUI_MOC_HDRS ${HEADERS})
-set(GUI_UIS CartesianWaypointControlGuiWidget.ui)
-set(COMPONENT_LIBS NJointControllerGuiPluginUtility)
-
-if(ArmarXGui_FOUND)
-    armarx_gui_library(CartesianWaypointControlGuiGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp
deleted file mode 100644
index fea32168c9cec6011c8e2c2143353baa80c38ea4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::CartesianWaypointControlGuiGuiPlugin
- * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date       2019
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "CartesianWaypointControlGuiGuiPlugin.h"
-
-#include "CartesianWaypointControlGuiWidgetController.h"
-
-namespace armarx
-{
-    CartesianWaypointControlGuiGuiPlugin::CartesianWaypointControlGuiGuiPlugin()
-    {
-        addWidget < CartesianWaypointControlGuiWidgetController > ();
-    }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h
deleted file mode 100644
index b4b523dcc7bf1e5e96b9c9a1e13c7692ab7c89d7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::CartesianWaypointControlGui
- * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date       2019
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-
-namespace armarx
-{
-    /**
-     * \class CartesianWaypointControlGuiGuiPlugin
-     * \ingroup ArmarXGuiPlugins
-     * \brief CartesianWaypointControlGuiGuiPlugin brief description
-     *
-     * Detailed description
-     */
-    class ARMARXCOMPONENT_IMPORT_EXPORT CartesianWaypointControlGuiGuiPlugin:
-        public armarx::ArmarXGuiPlugin
-    {
-        Q_OBJECT
-        Q_INTERFACES(ArmarXGuiInterface)
-        Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
-    public:
-        /**
-         * All widgets exposed by this plugin are added in the constructor
-         * via calls to addWidget()
-         */
-        CartesianWaypointControlGuiGuiPlugin();
-    };
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
deleted file mode 100644
index fef69f4366e204f0a27fa3857e0221d7d7dba2bb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
+++ /dev/null
@@ -1,731 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>CartesianWaypointControlGuiWidget</class>
- <widget class="QWidget" name="CartesianWaypointControlGuiWidget">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>907</width>
-    <height>739</height>
-   </rect>
-  </property>
-  <property name="windowTitle">
-   <string>CartesianWaypointControlGuiWidget</string>
-  </property>
-  <layout class="QGridLayout" name="gridLayout">
-   <item row="2" column="0" colspan="2">
-    <widget class="QGroupBox" name="groupBox_2">
-     <property name="title">
-      <string>Waypoints</string>
-     </property>
-     <property name="checkable">
-      <bool>true</bool>
-     </property>
-     <layout class="QGridLayout" name="gridLayout_4">
-      <property name="leftMargin">
-       <number>0</number>
-      </property>
-      <property name="topMargin">
-       <number>0</number>
-      </property>
-      <property name="rightMargin">
-       <number>0</number>
-      </property>
-      <property name="bottomMargin">
-       <number>0</number>
-      </property>
-      <item row="0" column="0" colspan="2">
-       <widget class="QWidget" name="widgetWPs" native="true">
-        <layout class="QGridLayout" name="gridLayout_6">
-         <item row="3" column="1">
-          <widget class="QPushButton" name="pushButtonStop">
-           <property name="text">
-            <string>Stop</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="2" rowspan="4">
-          <spacer name="verticalSpacer_2">
-           <property name="orientation">
-            <enum>Qt::Vertical</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>0</width>
-             <height>40</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
-         <item row="1" column="0" colspan="2">
-          <widget class="QTextEdit" name="textEditWPs"/>
-         </item>
-         <item row="3" column="0">
-          <widget class="QPushButton" name="pushButtonExecute">
-           <property name="text">
-            <string>Execute Waypoints</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="0" colspan="2">
-          <layout class="QHBoxLayout" name="horizontalLayout">
-           <item>
-            <widget class="QLabel" name="label_3">
-             <property name="sizePolicy">
-              <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-               <horstretch>0</horstretch>
-               <verstretch>0</verstretch>
-              </sizepolicy>
-             </property>
-             <property name="text">
-              <string>Format</string>
-             </property>
-            </widget>
-           </item>
-           <item>
-            <widget class="QRadioButton" name="radioButtonWPJson">
-             <property name="sizePolicy">
-              <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
-               <horstretch>0</horstretch>
-               <verstretch>0</verstretch>
-              </sizepolicy>
-             </property>
-             <property name="text">
-              <string>json</string>
-             </property>
-             <property name="checked">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item>
-            <widget class="QRadioButton" name="radioButton4f">
-             <property name="enabled">
-              <bool>false</bool>
-             </property>
-             <property name="sizePolicy">
-              <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
-               <horstretch>0</horstretch>
-               <verstretch>0</verstretch>
-              </sizepolicy>
-             </property>
-             <property name="text">
-              <string>Matrix4f</string>
-             </property>
-            </widget>
-           </item>
-           <item>
-            <widget class="QCheckBox" name="checkBoxWPReverse">
-             <property name="text">
-              <string>Reverse list</string>
-             </property>
-            </widget>
-           </item>
-           <item>
-            <spacer name="horizontalSpacer">
-             <property name="orientation">
-              <enum>Qt::Horizontal</enum>
-             </property>
-             <property name="sizeHint" stdset="0">
-              <size>
-               <width>40</width>
-               <height>20</height>
-              </size>
-             </property>
-            </spacer>
-           </item>
-           <item>
-            <widget class="QPushButton" name="pushButtonCopyCurrentPose">
-             <property name="text">
-              <string>copy current pose</string>
-             </property>
-            </widget>
-           </item>
-           <item>
-            <widget class="QLabel" name="labelParsingSuccess">
-             <property name="text">
-              <string/>
-             </property>
-            </widget>
-           </item>
-          </layout>
-         </item>
-        </layout>
-       </widget>
-      </item>
-     </layout>
-    </widget>
-   </item>
-   <item row="1" column="0" colspan="2">
-    <widget class="QGroupBox" name="groupBox_3">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="title">
-      <string>Settings</string>
-     </property>
-     <property name="checkable">
-      <bool>true</bool>
-     </property>
-     <layout class="QGridLayout" name="gridLayout_3">
-      <property name="leftMargin">
-       <number>0</number>
-      </property>
-      <property name="topMargin">
-       <number>0</number>
-      </property>
-      <property name="rightMargin">
-       <number>0</number>
-      </property>
-      <property name="bottomMargin">
-       <number>0</number>
-      </property>
-      <property name="spacing">
-       <number>0</number>
-      </property>
-      <item row="0" column="0">
-       <widget class="QWidget" name="widgetSettings" native="true">
-        <layout class="QGridLayout" name="gridLayout_5">
-         <item row="2" column="4">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxPosReached">
-           <property name="maximum">
-            <double>500.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>5.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="6">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxPosMaxVel">
-           <property name="maximum">
-            <double>500.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>80.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="0">
-          <widget class="QLabel" name="label_8">
-           <property name="text">
-            <string>Position</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="0">
-          <widget class="QLabel" name="label_15">
-           <property name="text">
-            <string>Orientation</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="5">
-          <widget class="QLabel" name="label_20">
-           <property name="text">
-            <string>Max vel</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="2">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxOriNear">
-           <property name="maximum">
-            <double>5.000000000000000</double>
-           </property>
-           <property name="singleStep">
-            <double>0.010000000000000</double>
-           </property>
-           <property name="value">
-            <double>0.100000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="1">
-          <widget class="QLabel" name="label_16">
-           <property name="text">
-            <string>Near</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="3">
-          <widget class="QLabel" name="label_17">
-           <property name="text">
-            <string>Reached</string>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="0">
-          <widget class="QLabel" name="label_6">
-           <property name="text">
-            <string>Joint limit avoidance</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="4">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxOriReached">
-           <property name="maximum">
-            <double>5.000000000000000</double>
-           </property>
-           <property name="singleStep">
-            <double>0.010000000000000</double>
-           </property>
-           <property name="value">
-            <double>0.050000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="1">
-          <widget class="QLabel" name="label_14">
-           <property name="text">
-            <string>Near</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="2">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxPosNear">
-           <property name="maximum">
-            <double>500.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>50.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="4">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccOri">
-           <property name="maximum">
-            <double>4.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>1.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="1">
-          <widget class="QLabel" name="label_9">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-           <property name="text">
-            <string>Pos</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="6">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccNull">
-           <property name="maximum">
-            <double>5.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>2.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="6">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxOriMaxVel">
-           <property name="maximum">
-            <double>5.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>1.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="0">
-          <widget class="QLabel" name="label_5">
-           <property name="text">
-            <string>Max acc</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="8">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxPosKP">
-           <property name="maximum">
-            <double>10.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>1.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="4">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxLimitAvoidScale">
-           <property name="maximum">
-            <double>10.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>2.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="1">
-          <widget class="QLabel" name="label_12">
-           <property name="text">
-            <string>KP</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="5">
-          <widget class="QLabel" name="label_11">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-           <property name="text">
-            <string>Nullspace</string>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="3">
-          <widget class="QLabel" name="label_13">
-           <property name="text">
-            <string>Scale</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="3">
-          <widget class="QLabel" name="label_10">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-           <property name="text">
-            <string>Ori</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="8">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxOriKP">
-           <property name="maximum">
-            <double>10.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>1.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="5">
-          <widget class="QLabel" name="label_19">
-           <property name="text">
-            <string>Max vel</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="2">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccPos">
-           <property name="maximum">
-            <double>2000.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>500.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="2">
-          <widget class="QDoubleSpinBox" name="doubleSpinBoxLimitAvoidKP">
-           <property name="maximum">
-            <double>10.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>1.000000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="8" column="0" colspan="9">
-          <layout class="QHBoxLayout" name="horizontalLayout_2">
-           <item>
-            <widget class="QPushButton" name="pushButtonZeroFT">
-             <property name="text">
-              <string>Zero FT</string>
-             </property>
-            </widget>
-           </item>
-           <item>
-            <widget class="QPushButton" name="pushButtonSendSettings">
-             <property name="text">
-              <string>Send</string>
-             </property>
-            </widget>
-           </item>
-          </layout>
-         </item>
-         <item row="3" column="3">
-          <widget class="QLabel" name="label_18">
-           <property name="text">
-            <string>Reached</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="7">
-          <widget class="QLabel" name="label_21">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-           <property name="text">
-            <string>Kp</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="7">
-          <widget class="QLabel" name="label_22">
-           <property name="text">
-            <string>Kp</string>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="0">
-          <widget class="QLabel" name="label_4">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-           <property name="text">
-            <string>FT limit</string>
-           </property>
-          </widget>
-         </item>
-         <item row="6" column="2" colspan="3">
-          <widget class="QCheckBox" name="checkBoxKeepOptimizing">
-           <property name="text">
-            <string>Optimize nullspace if target was reached</string>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="2" colspan="7">
-          <layout class="QHBoxLayout" name="horizontalLayout_3">
-           <item>
-            <widget class="QDoubleSpinBox" name="doubleSpinBoxFTLimit">
-             <property name="minimum">
-              <double>-1.000000000000000</double>
-             </property>
-             <property name="maximum">
-              <double>1000.000000000000000</double>
-             </property>
-             <property name="value">
-              <double>-1.000000000000000</double>
-             </property>
-            </widget>
-           </item>
-           <item>
-            <widget class="QCheckBox" name="checkBoxLimitinZ">
-             <property name="text">
-              <string>Limit along global Z</string>
-             </property>
-            </widget>
-           </item>
-          </layout>
-         </item>
-         <item row="7" column="2" colspan="6">
-          <widget class="QCheckBox" name="checkBoxSkipWaypoints">
-           <property name="text">
-            <string>Skip waypoints</string>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-        </layout>
-       </widget>
-      </item>
-     </layout>
-    </widget>
-   </item>
-   <item row="0" column="0" colspan="2">
-    <widget class="QGroupBox" name="groupBox">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="title">
-      <string>Controller Creation</string>
-     </property>
-     <property name="checkable">
-      <bool>true</bool>
-     </property>
-     <layout class="QGridLayout" name="gridLayout_2">
-      <property name="leftMargin">
-       <number>0</number>
-      </property>
-      <property name="topMargin">
-       <number>0</number>
-      </property>
-      <property name="rightMargin">
-       <number>0</number>
-      </property>
-      <property name="bottomMargin">
-       <number>0</number>
-      </property>
-      <item row="0" column="0">
-       <widget class="QWidget" name="widget" native="true">
-        <layout class="QGridLayout" name="gridLayout_7">
-         <property name="topMargin">
-          <number>0</number>
-         </property>
-         <property name="rightMargin">
-          <number>0</number>
-         </property>
-         <property name="bottomMargin">
-          <number>0</number>
-         </property>
-         <item row="1" column="0" rowspan="2">
-          <widget class="QLabel" name="label">
-           <property name="text">
-            <string>Kinematic Chain</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="0">
-          <widget class="QLabel" name="label_2">
-           <property name="text">
-            <string>FT Sensor (optional)</string>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="1">
-          <widget class="QComboBox" name="comboBoxChain"/>
-         </item>
-         <item row="0" column="2">
-          <widget class="QLabel" name="label_7">
-           <property name="text">
-            <string>Current</string>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="0">
-          <widget class="QPushButton" name="pushButtonCreateController">
-           <property name="text">
-            <string>Create</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="1">
-          <widget class="QLineEdit" name="lineEditFTName"/>
-         </item>
-         <item row="3" column="2">
-          <widget class="QLabel" name="labelCurrentControllerFT">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="2">
-          <widget class="QLabel" name="labelCurrentControllerChain">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-        </layout>
-       </widget>
-      </item>
-     </layout>
-    </widget>
-   </item>
-   <item row="3" column="0">
-    <widget class="QWidget" name="widgetSpacer" native="true">
-     <layout class="QVBoxLayout" name="verticalLayout">
-      <item>
-       <spacer name="verticalSpacer">
-        <property name="orientation">
-         <enum>Qt::Vertical</enum>
-        </property>
-        <property name="sizeHint" stdset="0">
-         <size>
-          <width>20</width>
-          <height>30</height>
-         </size>
-        </property>
-       </spacer>
-      </item>
-     </layout>
-    </widget>
-   </item>
-  </layout>
- </widget>
- <resources/>
- <connections>
-  <connection>
-   <sender>groupBox_2</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>widgetWPs</receiver>
-   <slot>setVisible(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>79</x>
-     <y>416</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>305</x>
-     <y>435</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>groupBox_3</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>widgetSettings</receiver>
-   <slot>setVisible(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>152</x>
-     <y>161</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>169</x>
-     <y>186</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>groupBox</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>widget</receiver>
-   <slot>setVisible(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>64</x>
-     <y>21</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>229</x>
-     <y>39</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>groupBox_2</sender>
-   <signal>clicked(bool)</signal>
-   <receiver>widgetSpacer</receiver>
-   <slot>setHidden(bool)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>151</x>
-     <y>400</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>14</x>
-     <y>721</y>
-    </hint>
-   </hints>
-  </connection>
- </connections>
-</ui>
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
deleted file mode 100644
index 303aa4ecdeabc7030e74b432858cb9c063b7f5bc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
+++ /dev/null
@@ -1,241 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package    RobotAPI::gui-plugins::CartesianWaypointControlGuiWidgetController
- * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date       2019
- * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include <random>
-#include <string>
-
-#include <QClipboard>
-
-#include <SimoxUtility/json.h>
-
-#include <ArmarXCore/util/CPPUtility/container.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "CartesianWaypointControlGuiWidgetController.h"
-
-namespace armarx
-{
-    CartesianWaypointControlGuiWidgetController::CartesianWaypointControlGuiWidgetController() :
-        NJointControllerGuiPluginBase("NJointCartesianWaypointController")
-    {
-        using T = CartesianWaypointControlGuiWidgetController;
-        std::lock_guard g{_allMutex};
-        _ui.setupUi(getWidget());
-        connect(_ui.radioButtonWPJson,         &QPushButton::clicked,   this, &T::triggerParsing);
-        connect(_ui.radioButton4f,             &QPushButton::clicked,   this, &T::triggerParsing);
-        connect(_ui.textEditWPs,               &QTextEdit::textChanged, this, &T::triggerParsing);
-
-        connect(_ui.pushButtonExecute,         &QPushButton::clicked,   this, &T::on_pushButtonExecute_clicked);
-        connect(_ui.pushButtonZeroFT,          &QPushButton::clicked,   this, &T::on_pushButtonZeroFT_clicked);
-        connect(_ui.pushButtonSendSettings,    &QPushButton::clicked,   this, &T::on_pushButtonSendSettings_clicked);
-        connect(_ui.pushButtonCopyCurrentPose, &QPushButton::clicked,   this, &T::copyCurrentPose);
-
-        connectCreateAcivateDeactivateDelete(
-            _ui.pushButtonCreateController,
-            nullptr,
-            _ui.pushButtonStop,
-            nullptr
-        );
-
-        _ui.widgetSpacer->setVisible(false);
-        _timer = startTimer(50);
-    }
-
-    CartesianWaypointControlGuiWidgetController::~CartesianWaypointControlGuiWidgetController()
-    {
-        if (_timer)
-        {
-            killTimer(_timer);
-        }
-    }
-
-    void CartesianWaypointControlGuiWidgetController::onConnectComponent()
-    {
-        std::lock_guard g{_allMutex};
-        NJointControllerGuiPluginBase::onConnectComponent();
-    }
-
-    void CartesianWaypointControlGuiWidgetController::createController()
-    {
-        std::lock_guard g{_allMutex};
-        NJointControllerGuiPluginBase::createController();
-        _supportsFT = !_ui.lineEditFTName->text().toStdString().empty();
-    }
-
-    void CartesianWaypointControlGuiWidgetController::on_pushButtonSendSettings_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (_controller)
-        {
-            ARMARX_IMPORTANT << "sending new config to " << getControllerName();
-            _controller->setConfig(readRunCfg());
-        }
-    }
-
-    void CartesianWaypointControlGuiWidgetController::copyCurrentPose()
-    {
-        std::lock_guard g{_allMutex};
-        if (!_robot)
-        {
-            return;
-        }
-        synchronizeLocalClone(_robot);
-        const auto rns = _robot->getRobotNodeSet(_ui.comboBoxChain->currentText().toStdString());
-        if (!rns)
-        {
-            return;
-        }
-        const auto tcp = rns->getTCP();
-        if (!tcp)
-        {
-            return;
-        }
-        const auto str = simox::json::eigen4f2posquatJson(tcp->getPoseInRootFrame());
-        QApplication::clipboard()->setText(QString::fromStdString(str));
-    }
-
-    void CartesianWaypointControlGuiWidgetController::on_pushButtonZeroFT_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (_controller && _supportsFT)
-        {
-            ARMARX_IMPORTANT << "setting ft offset for " << getControllerName();
-            _controller->setCurrentFTAsOffset();
-        }
-    }
-
-    void CartesianWaypointControlGuiWidgetController::on_pushButtonExecute_clicked()
-    {
-        std::lock_guard g{_allMutex};
-        if (_controller)
-        {
-            _controller->activateController();
-            ARMARX_IMPORTANT << "trigger execution of " << _lastParsedWPs.size()
-                             << " waypoints on " << getControllerName();
-            _controller->setWaypoints(_ui.checkBoxWPReverse->isChecked() ? reverse(_lastParsedWPs) : _lastParsedWPs);
-        }
-    }
-
-    void CartesianWaypointControlGuiWidgetController::triggerParsing()
-    {
-        std::lock_guard g{_allMutex};
-        _lastParsedWPs.clear();
-        _ui.labelParsingSuccess->setText("<pre parsing>");
-        if (_ui.radioButtonWPJson->isChecked())
-        {
-            //parse json
-            try
-            {
-                _lastParsedWPs = simox::json::posquatArray2eigen4fVector(
-                                     _ui.textEditWPs->toPlainText().toStdString());
-            }
-            catch (...)
-            {
-                _ui.labelParsingSuccess->setText("Failed to parse json array!");
-                return;
-            }
-        }
-        else if (_ui.radioButton4f->isChecked())
-        {
-            //parse lines
-            ///TODO parse Matrix4f style input
-            _ui.labelParsingSuccess->setText("NYI");
-            return;
-        }
-        //test reachability
-        {
-            ///TODO test reachability and visualize
-        }
-        _ui.labelParsingSuccess->setText("Parsed " + QString::number(_lastParsedWPs.size()) + " waypoints.");
-    }
-
-    NJointCartesianWaypointControllerRuntimeConfig CartesianWaypointControlGuiWidgetController::readRunCfg() const
-    {
-        std::lock_guard g{_allMutex};
-        NJointCartesianWaypointControllerRuntimeConfig cfg;
-
-        cfg.wpCfg.maxPositionAcceleration       = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
-        cfg.wpCfg.maxOrientationAcceleration    = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
-        cfg.wpCfg.maxNullspaceAcceleration      = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
-
-        cfg.wpCfg.kpJointLimitAvoidance         = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
-        cfg.wpCfg.jointLimitAvoidanceScale      = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
-
-        cfg.wpCfg.thresholdOrientationNear      = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
-        cfg.wpCfg.thresholdOrientationReached   = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
-        cfg.wpCfg.thresholdPositionNear         = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
-        cfg.wpCfg.thresholdPositionReached      = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
-
-        cfg.wpCfg.maxOriVel                     = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
-        cfg.wpCfg.maxPosVel                     = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
-        cfg.wpCfg.kpOri                         = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
-        cfg.wpCfg.kpPos                         = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
-
-        cfg.forceThreshold                      = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
-        cfg.optimizeNullspaceIfTargetWasReached = _ui.checkBoxKeepOptimizing->isChecked();
-        cfg.forceThresholdInRobotRootZ          = _ui.checkBoxLimitinZ->isChecked();
-        cfg.skipToClosestWaypoint               = _ui.checkBoxSkipWaypoints->isChecked();
-
-        return cfg;
-    }
-
-    void CartesianWaypointControlGuiWidgetController::timerEvent(QTimerEvent* e)
-    {
-        std::lock_guard g{_allMutex};
-        if (_robot)
-        {
-            synchronizeLocalClone(_robot);
-        }
-        if (_controller)
-        {
-            ARMARX_INFO << deactivateSpam() << "setting visu gp to:\n" << _robot->getGlobalPose();
-            _controller->setVisualizationRobotGlobalPose(_robot->getGlobalPose());
-        }
-    }
-
-    void CartesianWaypointControlGuiWidgetController::setupGuiAfterConnect()
-    {
-        bool found = false;
-        for (const auto& rnsn : _robot->getRobotNodeSetNames())
-        {
-            _ui.comboBoxChain->addItem(QString::fromStdString(rnsn));
-            if (rnsn == "RightArm")
-            {
-                _ui.comboBoxChain->setCurrentIndex(_ui.comboBoxChain->count() - 1);
-                found = true;
-            }
-        }
-        if (found && _robot->hasRobotNode("FT R"))
-        {
-            _ui.lineEditFTName->setText("FT R");
-        }
-    }
-
-    NJointControllerConfigPtr CartesianWaypointControlGuiWidgetController::readFullCFG() const
-    {
-        NJointCartesianWaypointControllerConfigPtr cfg = new NJointCartesianWaypointControllerConfig;
-        cfg->rns = _ui.comboBoxChain->currentText().toStdString();
-        cfg->ftName = _ui.lineEditFTName->text().toStdString();
-        cfg->runCfg = readRunCfg();
-        return NJointControllerConfigPtr::dynamicCast(cfg);
-    }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
deleted file mode 100644
index 01c59683c8dd3081e50ac58aa180d9c1a1d7ced0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::gui-plugins::CartesianWaypointControlGuiWidgetController
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2019
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
-#include <RobotAPI/gui-plugins/CartesianWaypointControlGui/ui_CartesianWaypointControlGuiWidget.h>
-
-namespace armarx
-{
-    /**
-    \page RobotAPI-GuiPlugins-CartesianWaypointControlGui CartesianWaypointControlGui
-    \brief The CartesianWaypointControlGui allows visualizing ...
-
-    \image html CartesianWaypointControlGui.png
-    The user can
-
-    API Documentation \ref CartesianWaypointControlGuiWidgetController
-
-    \see CartesianWaypointControlGuiGuiPlugin
-    */
-
-    /**
-     * \class CartesianWaypointControlGuiWidgetController
-     * \brief CartesianWaypointControlGuiWidgetController brief one line description
-     *
-     * Detailed description
-     */
-    class ARMARXCOMPONENT_IMPORT_EXPORT
-        CartesianWaypointControlGuiWidgetController:
-        public NJointControllerGuiPluginBase <
-        CartesianWaypointControlGuiWidgetController,
-        NJointCartesianWaypointControllerInterfacePrx
-        >
-    {
-        Q_OBJECT
-    public:
-        /// Controller Constructor
-        explicit CartesianWaypointControlGuiWidgetController();
-        ~CartesianWaypointControlGuiWidgetController();
-
-        /**
-         * Returns the Widget name displayed in the ArmarXGui to create an
-         * instance of this class.
-         */
-        static QString GetWidgetName()
-        {
-            return "RobotControl.NJointControllers.CartesianWaypointControlGui";
-        }
-
-        void onConnectComponent() override;
-        NJointControllerConfigPtr readFullCFG() const override;
-        void setupGuiAfterConnect() override;
-
-    private slots:
-        void on_pushButtonExecute_clicked();
-        void on_pushButtonZeroFT_clicked();
-        void on_pushButtonSendSettings_clicked();
-        void createController() override;
-        void copyCurrentPose();
-
-        void triggerParsing();
-
-    private:
-        NJointCartesianWaypointControllerRuntimeConfig readRunCfg() const;
-        void timerEvent(QTimerEvent*) override;
-
-    private:
-        Ui::CartesianWaypointControlGuiWidget           _ui;
-        std::vector<Eigen::Matrix4f>                    _lastParsedWPs;
-        bool                                            _supportsFT{false};
-        int                                             _timer{0};
-    };
-}
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
index 5ff2649fae12cb906f5821f73ba1fb1fd21647fe..df0f5596b78a783c94e6ae21fc1a0cfdf0e5a16e 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -13,7 +13,7 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * @package    Armar6Skills::ArmarXObjects::HumanAvoidance
+ * @package    armar6_skills::ArmarXObjects::HumanAvoidance
  * @author     Christian R. G. Dreher <c.dreher@kit.edu>
  * @author     Fabian Peller
  * @date       2019
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.cpp
deleted file mode 100644
index c03917a00653a626f9dfed22793699058a8139b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "AbstractData.h"
-
-using namespace armarx;
-
-AbstractData::AbstractData()
-{
-}
-
-AbstractData::~AbstractData()
-{
-
-}
-
-
-
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h
deleted file mode 100644
index 5123ac0df92bd4488e3f1c20fc8306af233d9eb4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h
+++ /dev/null
@@ -1,157 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#pragma once
-
-#include <memory>
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
-
-namespace armarx
-{
-    template<typename T>
-    class LinearConvertedValue
-    {
-    public:
-        LinearConvertedValue()
-        {
-            raw = nullptr;
-            offset = factor = 0;
-            this->offsetBeforeFactor = true;
-        }
-
-        /**
-         * @brief init
-         * @param raw
-         * @param node
-         * @param defaultValue
-         * @param offsetBeforeFactor if true the offset is added before multiplying with factor. If false: the other way around.
-         * @param nameForDebugging This name is printend in case an error is encountered (Its only purpose is to ease debugging)
-         */
-        void init(T* raw, const DefaultRapidXmlReaderNode& node, float defaultValue = std::nan("1"), bool offsetBeforeFactor = true, const char* nameForDebugging = "")
-        {
-            float factor = node.attribute_as_float("factor");
-            float offset = node.attribute_as_float("offset");
-            init(raw, factor, offset, defaultValue, offsetBeforeFactor, nameForDebugging);
-        }
-
-        void init(T* raw, float factor, float offset, float defaultValue = std::nan("1"), bool offsetBeforeFactor = true, const char* nameForDebugging = "")
-        {
-            const auto rawAsInt = reinterpret_cast<std::uint64_t>(raw);
-            ARMARX_CHECK_EXPRESSION(
-                (rawAsInt % alignof(T)) == 0) <<
-                                              "\nThe alignment is wrong!\nIt has to be "
-                                              << alignof(T) << ", but the data is aligned with "
-                                              << rawAsInt % alignof(std::max_align_t)
-                                              << "!\nThis is an offset of " << (rawAsInt % alignof(T))
-                                              << " bytes!\nThe datatype is " << GetTypeString<T>()
-                                              << "\nIts size is " << sizeof(T)
-                                              << "\nraw = " << raw
-                                              << " bytes\nThe name is " << nameForDebugging;
-            this->offsetBeforeFactor = offsetBeforeFactor;
-            this->factor = factor;
-            this->offset = offset;
-            this->raw = raw;
-            if (!std::isnan(defaultValue))
-            {
-                value = defaultValue;
-                write();
-            }
-            else
-            {
-                value = 0;
-                read();
-            }
-        }
-
-        void read()
-        {
-            if (offsetBeforeFactor)
-            {
-                value = ((*raw) + offset) * factor;
-            }
-            else
-            {
-                value = (*raw) * factor + offset;
-            }
-        }
-
-        void write()
-        {
-            if (offsetBeforeFactor)
-            {
-                *raw = (T)((value / factor) - offset);
-            }
-            else
-            {
-                *raw = (T)((value) - offset) / factor;
-            }
-        }
-
-        float value;
-
-        T getRaw() const
-        {
-            return *raw;
-        }
-
-        float getFactor() const
-        {
-            return factor;
-        }
-
-        float getOffset() const
-        {
-            return offset;
-        }
-
-        bool getOffsetBeforeFactor() const
-        {
-            return offsetBeforeFactor;
-        }
-
-    private:
-        T* raw;
-        float offset, factor;
-        bool offsetBeforeFactor ;
-    };
-
-    class AbstractData;
-    using AbstractDataPtr = std::shared_ptr<AbstractData>;
-
-
-    class AbstractData
-    {
-    public:
-        AbstractData();
-        virtual ~AbstractData();
-        virtual void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
-        virtual void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
-
-    private:
-    };
-
-}
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.cpp
deleted file mode 100644
index 7d5db2ec6d4fdf8af417d6ad56fe07adc38fcb7d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#include "AbstractFunctionalDevice.h"
-
-bool armarx::AbstractFunctionalDevice::isInitialized() const
-{
-    return initialized;
-}
-
-std::string armarx::AbstractFunctionalDevice::getClassName() const
-{
-    return className;
-}
-
-const armarx::DefaultRapidXmlReaderNode armarx::AbstractFunctionalDevice::getNode() const
-{
-    return node;
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
deleted file mode 100644
index 0bbbf7011e67bcb59c3e1dacbc0e5fe5541f1221..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#pragma once
-
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h>
-
-namespace armarx
-{
-    class AbstractFunctionalDevice;
-    using AbstractFunctionalDevicePtr = std::shared_ptr<AbstractFunctionalDevice>;
-
-    class AbstractFunctionalDevice : public std::enable_shared_from_this<AbstractFunctionalDevice>
-    {
-    public:
-        AbstractFunctionalDevice(DefaultRapidXmlReaderNode configNode) :
-            node(configNode),
-            initialized(false)
-        {
-            //just to be sure, sometimes strange things happen when don't do it
-            node = configNode;
-        }
-        virtual ~AbstractFunctionalDevice() {}
-
-        virtual const DefaultRapidXmlReaderNode getNode() const;
-
-        virtual bool isInitialized() const;
-
-        /**
-         * This converts the sensor data that arrive from the bus into floats and copys them they can be published via a DataUnit.
-         */
-        virtual void initData() = 0;
-        virtual void execute() {}
-
-
-        std::string getClassName() const;
-
-    protected:
-        template <typename Base, typename constructorArg, typename SharedPointer>
-        friend class AbstractFactoryMethod;
-        std::string className;
-        DefaultRapidXmlReaderNode node;
-        bool initialized = false;
-    };
-}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.cpp
deleted file mode 100644
index 0995327f5367fb0a9835bf57911642dd64af88d1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-//
-// Created by swarowsky on 21.12.16.
-//
-
-#include "AbstractSlave.h"
-#include "EtherCAT.h"
-
-using namespace armarx;
-
-AbstractSlave::AbstractSlave(const SlaveIdentifier slaveIdentifier, uint16_t slaveNumber)
-    : slaveIdentifier(slaveIdentifier), slaveNumber(slaveNumber)
-{
-
-}
-
-
-///**
-// * Returns the Vendor ID of the slave
-// * @return the vendor id of the slave
-// */
-//uint32_t AbstractSlave::getVendorID() const
-//{
-//    return vendorID;
-//}
-
-/**
- * This returns the slave number of the slave on the bus +1 because slave 0 is the master
- * @return index in ec_slave array
- */
-uint16_t AbstractSlave::getSlaveNumber() const
-{
-    return slaveNumber;
-}
-
-//uint32_t AbstractSlave::getSerialNumber() const
-//{
-//    return serialNumber;
-//}
-
-bool AbstractSlave::handleErrors()
-{
-    bool retVal;
-    retVal = !hasError();
-    return retVal;
-}
-
-const SlaveIdentifier& AbstractSlave::getSlaveIdentifier() const
-{
-    return slaveIdentifier;
-}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h
deleted file mode 100644
index f9064b08c179e984fb7eb6cd9f95e32c5e96ae76..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h
+++ /dev/null
@@ -1,160 +0,0 @@
-#pragma once
-
-#include <stdint.h>
-#include <memory>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "SlaveIdentifier.h"
-
-
-namespace armarx
-{
-    class EtherCAT;
-
-#define DEFAULT_VENDOR_ID 0
-
-    class AbstractSlave;
-    using AbstractSlavePtr = std::shared_ptr<AbstractSlave>;
-
-    class AbstractSlave : public armarx::Logging
-    {
-
-    public:
-        AbstractSlave(const armarx::SlaveIdentifier slaveIdentifier, uint16_t slaveNumber);
-        virtual ~AbstractSlave() {}
-        /**
-         * This is called after EtherCAT Bus is PreOp Mode. This is where the PDO Mapping can be configured for the slave.
-         */
-        virtual void doMappings() = 0;
-
-        /**
-         * This gets triggered by the bus controller before it will start the control loop.
-         * If a slave needs more preparation than just getting in EtherCAT Op-Mode this should be done here.
-         * So slaves can assume that the EtherCAT state machine is in Op-Mode so PDO's are available.
-         * Attention!!! This needs to be implemented cooperative
-         * @return true if the prepare is finished an don't needs to be called again
-         */
-        virtual bool prepare() = 0;
-        /**
-         * This method gets triggered by the Bus Controller, this function hast to be implemented cooperative.
-         * The Bus controller will guarantee that the process data will be update before each call.
-         */
-        virtual void execute() = 0;
-
-        /**
-         * This gets triggered by the bus Controller before it will close the EtherCAT bus.
-         * So if the device need to do something before to get in a safe state, this can be done here.
-         * Attention!!! This needs to be implemented cooperative
-         * @return if slave is shut down
-         */
-        virtual bool shutdown()  = 0;
-
-        virtual void setInputPDO(void* ptr) = 0;
-
-        virtual void setOutputPDO(void* ptr) = 0;
-
-        /**
-         * This gets called between the SafeOp an the Op state of the bus at the initizialisation
-         */
-        virtual void prepareForOp() = 0;
-        /**
-         * @brief This gets called after prepareForOp() was called. This is useful if prepareForOp()
-         * executes a long running initialization and needs to be done before the slave goes into op
-         */
-        virtual void finishPreparingForOp() {}
-
-        virtual void prepareForSafeOp() {}
-        virtual void finishPreparingForSafeOp() {}
-
-        /**
-         * This function indicates if there is a error or Problem with this slave. It should not
-         * @return true if there is an error/problem with this slave otherwise false;
-         */
-        virtual bool hasError() = 0;
-        virtual bool isEmergencyStopActive() const
-        {
-            return false;
-        }
-        virtual bool recoverFromEmergencyStop()
-        {
-            return true;
-        }
-
-        /**
-         * This tries to clear oder fix the errors or problems of the slave or just gives detailed information about the problem.
-         * If hasError == false this function does nothing.
-         * @return true if the function is trying to recover the slave or there is no error, false is send if this just reports the info
-         */
-        virtual bool handleErrors();
-
-        /*uint32_t getVendorID() const ;
-
-        uint32_t getSerialNumber() const;*/
-
-        uint16_t getSlaveNumber() const;
-        const SlaveIdentifier& getSlaveIdentifier() const;
-
-        virtual bool hasPDOMapping() const = 0;
-
-    private:
-        const armarx::SlaveIdentifier slaveIdentifier;
-        /*const uint32_t vendorID;
-        const uint32_t serialNumber;*/
-        const uint16_t slaveNumber;
-
-    };
-
-    template<class InputT, class OutputT>
-    class AbstractSlaveWithInputOutput : public AbstractSlave
-    {
-    public:
-        using AbstractSlave::AbstractSlave;
-
-        void setInputPDO(void* ptr) override
-        {
-            const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr);
-            ARMARX_CHECK_EXPRESSION(
-                (ptrAsInt % alignof(InputT)) == 0) <<
-                                                   "\nThe alignment is wrong!\nIt has to be "
-                                                   << alignof(InputT) << ", but the data is aligned with "
-                                                   << ptrAsInt % alignof(std::max_align_t)
-                                                   << "!\nThis is an offset of " << (ptrAsInt % alignof(InputT))
-                                                   << " bytes!\nThe datatype is " << GetTypeString<InputT>()
-                                                   << "\nIts size is " << sizeof(InputT)
-                                                   << " bytes";
-            inputs = static_cast<InputT*>(ptr);
-        }
-        void setOutputPDO(void* ptr) override
-        {
-            const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr);
-            ARMARX_CHECK_EXPRESSION(
-                (ptrAsInt % alignof(OutputT)) == 0) <<
-                                                    "\nThe alignment is wrong!\nIt has to be "
-                                                    << alignof(OutputT) << ", but the data is aligned with "
-                                                    << ptrAsInt % alignof(std::max_align_t)
-                                                    << "!\nThis is an offset of " << (ptrAsInt % alignof(OutputT))
-                                                    << " bytes!\nThe datatype is " << GetTypeString<OutputT>()
-                                                    << "\nIts size is " << sizeof(OutputT)
-                                                    << " bytes";
-            outputs = static_cast<OutputT*>(ptr);
-        }
-        bool hasPDOMapping() const final override
-        {
-            return true;
-        }
-        OutputT* getOutputsPtr()
-        {
-            return outputs;
-        }
-        InputT* getInputsPtr()
-        {
-            return inputs;
-        }
-    protected:
-        InputT* inputs{nullptr};
-        OutputT* outputs;
-
-    };
-
-}
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/ArmarXEtherCATFwd.h b/source/RobotAPI/libraries/ArmarXEtherCAT/ArmarXEtherCATFwd.h
deleted file mode 100644
index f321e2de7af4a47a60a31e9b7a3cd0f19b2687e2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/ArmarXEtherCATFwd.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <memory>
-#include <Ice/Handle.h>
-
-#define H2T_VENDOR_ID 0x7bc
-#define H2T_SENSOBOARD_PRODUCT_CODE 0x9252
-
-namespace armarx
-{
-    using DeviceContainerPtr = std::shared_ptr<class DeviceContainer>;
-    class EtherCAT;
-
-
-    class AbstractSlave;
-    using AbstractSlavePtr = std::shared_ptr<AbstractSlave>;
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt
deleted file mode 100644
index 3e54ff4f9b66febc9f1403e62fa194262850a275..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt
+++ /dev/null
@@ -1,41 +0,0 @@
-armarx_component_set_name("ArmarXEtherCAT")
-armarx_set_target("Library: ArmarXEtherCAT")
-
-set(LIB_NAME       ArmarXEtherCAT)
-
-find_package(SOEM QUIET)
-armarx_build_if(SOEM_FOUND "SOEM not available")
-
-set(LIBS RobotUnit ${SOEM_LIBRARIES})
-
-set(LIB_FILES
-    EtherCAT.cpp
-    DeviceContainer.cpp
-    AbstractFunctionalDevice.cpp
-    AbstractSlave.cpp
-    AbstractData.cpp
-    EtherCATDeviceFactory.cpp
-    SlaveIdentifier.cpp
-    EtherCATRTUnit.cpp
-    )
-set(LIB_HEADERS
-    ArmarXEtherCATFwd.h
-    EtherCAT.h
-    DeviceContainer.h
-    AbstractFunctionalDevice.h
-    AbstractSlave.h
-    AbstractData.h
-    EtherCATDeviceFactory.h
-    SlaveIdentifier.h
-    EtherCATRTUnit.h
-    VirtualDeviceFactory.h
-    )
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-if (SOEM_FOUND)
-    target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${SOEM_INCLUDE_DIR})
-    target_link_libraries("${LIB_NAME}" PUBLIC ${SOEM_LIBRARIES})
-endif()
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp
deleted file mode 100644
index 3ba0cd1a3b3e45be76353f5357894beff10b7163..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#include "DeviceContainer.h"
-#include "AbstractFunctionalDevice.h"
-
-#include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
-#include <VirtualRobot/Robot.h>
-
-
-namespace armarx
-{
-
-    size_t DeviceContainer::load(const MultiNodeRapidXMLReader& rootNodeConfigs, const VirtualRobot::RobotPtr& robot)
-    {
-        //        sleep(5);
-        size_t addedDevices = 0;
-        //        auto children = robot->getRobotNodes();
-        //        auto getSceneObject = [&](const std::string & name)
-        //        {
-        //            for (auto& obj : children)
-        //            {
-        //                if (obj->getName() == name)
-        //                {
-        //                    return VirtualRobot::SceneObjectPtr(obj);
-        //                }
-        //            }
-        //            for (auto& s : robot->getSensors())
-        //            {
-        //                if (s->getName() == name)
-        //                {
-        //                    return  VirtualRobot::SceneObjectPtr(s);
-        //                }
-        //            }
-        //            return VirtualRobot::SceneObjectPtr();
-        //        };
-        //rootNode = rootNodeConfig;
-        ARMARX_DEBUG << "Device factories: " << VirtualDeviceFactory::getAvailableClasses();
-        auto defaultNode = DefaultRapidXmlReaderNode(rootNodeConfigs.nodes("DefaultConfiguration"));
-        for (auto& node : rootNodeConfigs.nodes(nullptr))
-        {
-            try
-            {
-
-
-                if (node.name() == "DefaultConfiguration" || node.name() == "include" || node.name().empty())
-                {
-                    continue;
-                }
-                auto name = node.attribute_value("name");
-                ARMARX_DEBUG << "Handling: " << node.name() << " name: " << name;
-                //            auto obj = getSceneObject(name);
-                //            ARMARX_CHECK_EXPRESSION(obj) << name;
-                auto tuple = std::make_tuple(node, defaultNode, robot);
-                auto instance = VirtualDeviceFactory::fromName(node.name(), tuple);
-                if (!instance)
-                {
-                    ARMARX_WARNING << "No factory found for virtual device " << node.name();
-                }
-                else
-                {
-                    ARMARX_VERBOSE << "Created instance of type " << node.name();
-                    devices.push_back(instance);
-                    addedDevices++;
-                }
-            }
-            catch (...)
-            {
-                handleExceptions();
-            }
-        }
-        return addedDevices;
-
-    }
-
-    std::vector<AbstractFunctionalDevicePtr> DeviceContainer::getAllInitializedFunctionalDevices() const
-    {
-        std::vector<AbstractFunctionalDevicePtr> returnList;
-        for (AbstractFunctionalDevicePtr& device : getAllFunctionalDevices())
-        {
-            if (device && device->isInitialized())
-            {
-                returnList.push_back(device);
-            }
-        }
-        return returnList;
-    }
-
-    std::vector<AbstractFunctionalDevicePtr> DeviceContainer::getAllUninitializedFunctionalDevices() const
-    {
-        std::vector<AbstractFunctionalDevicePtr> returnList;
-        for (AbstractFunctionalDevicePtr& device : getAllFunctionalDevices())
-        {
-            if (device && !device->isInitialized())
-            {
-                returnList.push_back(device);
-            }
-        }
-        return returnList;
-    }
-
-    std::vector<AbstractFunctionalDevicePtr> DeviceContainer::getAllFunctionalDevices() const
-    {
-        return devices;
-    }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
deleted file mode 100644
index 538c1b5cb5c5b47222b97f13d6417706d4b8a746..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-#include <vector>
-#include <memory>
-#include <VirtualRobot/VirtualRobot.h>
-
-// These includes are only here because Armar6RT is currently locked (missing include in Slave.cpp)
-// TODO: Remove once Armar6RT includes the needed headers
-#include <boost/core/demangle.hpp>
-#include <boost/optional.hpp>
-
-namespace armarx
-{
-    class MultiNodeRapidXMLReader;
-    using AbstractFunctionalDevicePtr = std::shared_ptr<class AbstractFunctionalDevice>;
-
-    class DeviceContainer
-    {
-    public:
-        size_t load(const MultiNodeRapidXMLReader& rootNodeConfigs, const VirtualRobot::RobotPtr& robot);
-        template <typename Type>
-        std::vector<std::shared_ptr<Type>> getDevicesOfType() const
-        {
-            std::vector<std::shared_ptr<Type>> results;
-            for (auto& dev : devices)
-            {
-                auto castedDev = std::dynamic_pointer_cast<Type>(dev);
-                if (castedDev)
-                {
-                    results.push_back(castedDev);
-                }
-
-            }
-            return results;
-        }
-        std::vector<AbstractFunctionalDevicePtr> getAllInitializedFunctionalDevices() const;
-        std::vector<AbstractFunctionalDevicePtr> getAllUninitializedFunctionalDevices() const;
-        virtual std::vector<AbstractFunctionalDevicePtr> getAllFunctionalDevices() const;
-    protected:
-        std::vector<AbstractFunctionalDevicePtr> devices;
-    };
-
-
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
deleted file mode 100644
index bb239e656673d84e2849ae1485de41f744889ccf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ /dev/null
@@ -1,1591 +0,0 @@
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wpedantic"
-
-#pragma GCC diagnostic pop
-
-#include <ethercat.h>
-#include <ctime>
-#include <chrono>
-#include <thread>
-#include <iomanip>
-
-#include <ArmarXCore/core/logging/Logging.h>
-//#include <Armar6RT/units/Armar6Unit/FunctionalDevices/Joint.h>
-//#include "Armar6RT/units/Armar6Unit/BusSlaves/SensorBoard.h"
-//#include "Armar6RT/units/Armar6Unit/BusSlaves/Elmo.h"
-//#include "Armar6RT/units/Armar6Unit/BusSlaves/FTSensorSlave.h"
-//#include <Armar6RT/units/Armar6Unit/BusSlaves/EtherCATHub.h>
-#include "EtherCAT.h"
-#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
-//#include "RtRobotContainer.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "AbstractSlave.h"
-#include "DeviceContainer.h"
-#include "EtherCATDeviceFactory.h"
-//include all soem stuff here so no one should be able to use soem functions unintentionally because he has to include some first
-#define EC_VER1
-#include <ethercattype.h>
-#include <nicdrv.h>
-#include <osal.h>
-#include <osal_defs.h>
-#include <ethercatmain.h>
-#include <ethercatbase.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-
-//EtherCAT definitions only used for the ethercat stuff
-#define SDO_READ_TIMEOUT 100000
-#define SDO_WRITE_TIMEOUT 50000
-#define EC_TIMEOUTMON 2000
-
-using namespace armarx;
-
-
-
-
-#define DEFAULT_ECAT_GROUP 0
-
-
-/**
- * This returns the one and only Bus object.
- * An implements the singelton pattern
- * @return The Bus instance
- */
-EtherCAT& EtherCAT::getBus()
-{
-    static EtherCAT _instance;
-    return _instance;
-}
-
-/**
- * default constructor, is privat an only be used from EtherCAT::getBus(), because of singelton.
- * @see EtherCAT::getBus()
- */
-EtherCAT::EtherCAT() : expectedWKC(-1),
-    isSDOWorking(false),
-    switchON_OFF(OFF),
-    currentGroup(DEFAULT_ECAT_GROUP),
-    error(false),
-    busInOperationalMode(false),
-    lastWorkCounter(0),
-    functionalDevices(),
-    actualMappedSize(0),
-    isBusDead(false),
-    pdoMapped(false),
-    deviceContainerPtr(nullptr),
-    mainUnitPtr(nullptr)
-{
-    checkErrorCountersOnWKCError = false;
-    //writing zeros to the IOMap
-    for (size_t i = 0; i < IOmapSize; ++i)
-    {
-        IOmap[i] = 0;
-    }
-    //for the start we don't need the recovery part to be working
-    ec_group[currentGroup].docheckstate = FALSE;
-}
-
-/**
- * This starts the bus by connection to the socket and initialize all the slaves, you will not be able to use the bus without calling this method before.
- * @param [IN] ifname the socket the bus should connect to
- * @return true if the bus could be started, false if something went wrong
- */
-bool EtherCAT::startBus(bool createDevices)
-{
-    ARMARX_TRACE;
-    //if the bus runs already do nothing
-    if (isSDOWorking)
-    {
-        return true;
-    }
-
-    if (socketFileDescriptor == -1)
-    {
-        ARMARX_WARNING << "socketFileDescriptor is -1 - did you forget to set it?";
-        error = true;
-        return false;
-    }
-
-    if (!ifname.empty())
-    {
-        ARMARX_IMPORTANT << "ec_init(" << ifname << ")";
-        if (!ec_init(ifname.c_str()))
-        {
-            ARMARX_WARNING << "Could not init etherCAT on " << ifname << "\nExecute as root\n";
-            error = true;
-            //end here there is nothing we can do
-            return false;
-        }
-    }
-    else if (socketFileDescriptor != -1)
-    {
-        ARMARX_INFO << "Using socketFileDescriptor " << socketFileDescriptor << " to open raw socket";
-        //initialise SOEM, open socket
-        if (!ec_init_wsock(socketFileDescriptor))
-        {
-            ARMARX_WARNING << "No socket connection on " << socketFileDescriptor << "\nExecute as root\n";
-            error = true;
-            //end here there is nothing we can do
-            return false;
-        }
-    }
-    else
-    {
-        ARMARX_WARNING << "Either socketFileDescriptor or ifname need to be set";
-        error = true;
-        //end here there is nothing we can do
-        return false;
-    }
-
-
-    //We succeed
-    ARMARX_INFO << "Started SOEM with socketFileDescriptor: " << socketFileDescriptor << std::endl;
-
-    //config Bus to switch to to Pre-OP
-    if (ec_config_init(FALSE) <= 0)
-    {
-        ARMARX_WARNING << "No slaves found! - close socket\n";
-        //stop soem
-        ec_close();
-        isSDOWorking = false;
-        return false;
-    }
-
-    ARMARX_TRACE;
-    //wait to be sure
-    osal_usleep(500000);
-    //we are up and running for SDO's
-    isSDOWorking = true;
-    //slaves should be in PreOp mode now
-    ARMARX_INFO << ec_slavecount << " slaves found and set to PreOp";
-
-    std::vector<ControlDevicePtr> ctrlDevs;
-    std::vector<SensorDevicePtr> sensDevs;
-
-    ////prepare Devices to be read to switch to Safe-Op
-    if (createDevices)
-    {
-        if (slaves.size() > 0)
-        {
-            ARMARX_ERROR << "Devices are already created - stopping starting bus";
-            return false;
-        }
-        std::tie(ctrlDevs, sensDevs) = this->createDevices();
-
-        if (slaves.size() < 1)
-        {
-            ARMARX_WARNING << "There are no usable devices on the bus!";
-            return false;
-        }
-        ARMARX_INFO << "Devices where created";
-        for (std::shared_ptr<AbstractSlave>& slave : slaves)
-        {
-            slave->doMappings();
-        }
-    }
-    else if (slaves.size() < 1)
-    {
-        ARMARX_ERROR << "No Slaves configured - stopping bus start up";
-        return false;
-    }
-
-
-
-
-    ARMARX_TRACE;
-    for (auto slave : slaves)
-    {
-        slave->prepareForSafeOp();
-    }
-    ARMARX_INFO << "Finishing Preparing for safe op now";
-    for (auto slave : slaves)
-    {
-        slave->finishPreparingForSafeOp();
-    }
-
-    osal_usleep(500000);
-
-    ///// going to SAFE_OP
-    //do the mapping
-    ARMARX_INFO << "Mapping....";
-    actualMappedSize = ec_config_map(IOmap.data());
-    ARMARX_INFO << "Going to safe op now";
-    //wait to get all slaves to SAFE-OP
-    ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
-    ARMARX_INFO << "IOmapping done, size: " << actualMappedSize << " - all Slaves are in SAFE-OP now\n";
-
-    //calculating Workcounter after mapping to have an error indication later
-    expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
-    ARMARX_INFO << "Calculated workcounter: " << expectedWKC << std::endl;
-
-    ///give the devices her mapping
-    if (!setPDOMappings())
-    {
-        ARMARX_ERROR << "Couldn't map the PDO, may the the pc is under to much load. "
-                     "Check if there are other performance hungry programs running.\n"
-                     "Or just try to start again";
-        return false;
-    }
-
-    ///give the slaves some time to prepare some stuff
-    for (auto slave : slaves)
-    {
-        slave->prepareForOp();
-        //        updateBus();
-    }
-
-
-    for (auto slave : slaves)
-    {
-        slave->finishPreparingForOp();
-    }
-
-
-    /// init functional devices
-    functionalDevices = deviceContainerPtr->getAllInitializedFunctionalDevices();
-    ARMARX_INFO << "Found " << functionalDevices.size() << " meta devices";
-    /// setting the data pointer in the functional devices
-    for (AbstractFunctionalDevicePtr& device : functionalDevices)
-    {
-        ARMARX_INFO << "init for device type '" << device->getClassName() << "'";
-        device->initData();
-    }
-
-    this->ctrlDevs = ctrlDevs;
-    this->sensDevs = sensDevs;
-
-
-    pdoMapped = true;
-
-    //// switching to OP-Mode
-    ec_slave[0].state = EC_STATE_OPERATIONAL;
-    //send one valid process data to make outputs in slaves happy
-    ec_send_processdata();
-    ec_receive_processdata(EC_TIMEOUTRET);
-
-    ARMARX_TRACE;
-    //request all slaves to transit to OP-Mode
-    int stateRet = ec_writestate(0);
-    if (stateRet == 1)
-    {
-        ARMARX_WARNING << " ec_writestate FAILED - " << stateRet << std::endl;
-    }
-    else
-    {
-        ARMARX_INFO << "ec_writestate WORKING\n";
-    }
-    int chk = 40;
-    // wait for all slaves to reach OP state
-    // send the pdo at least one time, so the slaves see we are there up and running
-    do
-    {
-        ec_send_processdata();
-        ec_receive_processdata(EC_TIMEOUTRET);
-        ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
-        ARMARX_INFO << deactivateSpam(3) << "Waiting for slaves to reach operational state";
-    }
-    while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
-
-    //check if we succeeded
-    if (ec_slave[0].state != EC_STATE_OPERATIONAL)
-    {
-        ARMARX_ERROR << "Not all slaves reached operational state. Slaves that are not in operational state:" << std::endl;
-        ec_readstate();
-        //looking for errors
-        for (uint16 i = 1; i <= ec_slavecount; i++)
-        {
-            if (ec_slave[i].state != EC_STATE_OPERATIONAL)
-            {
-                printALStatusError(i);
-            }
-        }
-
-        //returning with an error
-        return false;
-    }
-    ARMARX_TRACE;
-    ARMARX_INFO << "All Slaves in OP now!" << std::endl;
-
-
-    //preparing devices to run
-    size_t slaveReadyCounter = 0;
-    while (switchON_OFF == ON && slaveReadyCounter != slaves.size())
-    {
-        slaveReadyCounter = 0;
-        std::string missingSlaves;
-        for (AbstractSlavePtr& slave : slaves)
-        {
-            if (slave->prepare())
-            {
-                slaveReadyCounter++;
-            }
-            else
-            {
-                missingSlaves += slave->getSlaveIdentifier().humanName + ", ";
-            }
-        }
-        ARMARX_INFO << deactivateSpam(1) << "Waiting for " << (slaves.size() - slaveReadyCounter) << "/" << slaves.size() << " slaves to get ready: " << missingSlaves;
-        updatePDO();
-    }
-    ARMARX_TRACE;
-    ARMARX_DEBUG << "PDOs updated.";
-    std::stringstream slaveInfo;
-    for (AbstractSlavePtr& slave : slaves)
-    {
-        slaveInfo << "#" << slave->getSlaveNumber() << ": " << slave->getSlaveIdentifier().humanName << "\n";
-    }
-    ARMARX_VERBOSE << "Used slaves: \n" << slaveInfo.str();
-    //check if the bus was put up in op mode or if it was switched off
-    if (switchON_OFF == OFF)
-    {
-        return false;
-    }
-
-    //all slaves ar in Op - not only EtherCAT Op also DS 402 for the elmos
-    busInOperationalMode = true;
-    busUpdateLastUpdateTime = IceUtil::Time::now(IceUtil::Time::Monotonic);
-    //if we get here all went good,
-    return true;
-}
-
-/**
- * This checks if the bus was already started an if not it will print an error message.
- * With this method you don't need to spread the error message all over the code. Just using isStared gives the same result
- * but without any message.
- * @see isWorking
- * @return true if bus was already started othewise false
- */
-bool EtherCAT::busShouldBeRunning() const
-{
-    if (!isSDOWorking)
-    {
-        ARMARX_INFO << "Bus isn't started yet! So the bus is not available !!!! \n”";
-        return false;
-    }
-    return true;
-}
-
-std::string ec_errorToString(ec_errort const&  error)
-{
-    std::stringstream result;
-    result << VAROUT(error.Etype) << "\n" << VAROUT(error.Signal) << "\n"
-           << VAROUT(error.Slave) << "\n"
-           << VAROUT(error.Index) << "\n"
-           << VAROUT(error.SubIdx) << "\n"
-           << "\n";
-    return result.str();
-}
-
-/**
- * This sets the pointers of the PDO mappings for the devices>
- */
-bool EtherCAT::setPDOMappings()
-{
-    ARMARX_TRACE;
-    bool retVal = true;
-    int byteSum = 0;
-    for (std::shared_ptr<AbstractSlave>& slave : slaves)
-    {
-        ARMARX_INFO << "Checking mapping for slave " << slave->getSlaveNumber() << " name: " << slave->getSlaveIdentifier().humanName;
-        if (!slave->hasPDOMapping())
-        {
-            ARMARX_INFO << "No mapping needed for " << slave->getSlaveIdentifier().humanName;
-            continue;
-        }
-        byteSum += ec_slave[slave->getSlaveNumber()].Obytes + ec_slave[slave->getSlaveNumber()].Ibytes;
-        if (ec_slave[slave->getSlaveNumber()].outputs == nullptr || ec_slave[slave->getSlaveNumber()].inputs == nullptr)
-        {
-            ARMARX_ERROR << "There is a nullpointer in the Mapping of Slave " << slave->getSlaveNumber();
-
-            ARMARX_IMPORTANT << "current Slave" << slave->getSlaveNumber();
-            ARMARX_IMPORTANT << "slaveCount: " << ec_slavecount;
-            //            ARMARX_IMPORTANT << "size in: " << sizeof(ELMO_in_t) << " size out: " << sizeof(ELMO_out_t);
-            ARMARX_IMPORTANT << "iomap ptr: 0x" << std::hex << &IOmap << std::dec;
-            ARMARX_IMPORTANT << "sn:" << slave->getSlaveNumber() << std::flush;
-            ARMARX_IMPORTANT << "out: 0x" << std::hex << (long)(ec_slave[slave->getSlaveNumber()].outputs) << std::dec
-                             << std::flush;
-            ARMARX_IMPORTANT << "in: 0x" << std::hex << (long)(ec_slave[slave->getSlaveNumber()].inputs) << std::dec
-                             << std::flush;
-            ARMARX_IMPORTANT << "in diff:  " << (long)(ec_slave[slave->getSlaveNumber()].inputs - ec_slave[0].outputs)
-                             << std::flush;
-            ARMARX_IMPORTANT << "-------------------------------------------------------";
-            ec_errort error_type;
-            while (ec_poperror(&error_type))
-            {
-                ARMARX_WARNING << "SOEM ERROR: " << ec_errorToString(error_type);
-            }
-            retVal = false;
-        }
-        else
-        {
-            //setting pdo mappings slave inputs are master outputs and vice versa
-            slave->setInputPDO(ec_slave[slave->getSlaveNumber()].outputs);
-            slave->setOutputPDO(ec_slave[slave->getSlaveNumber()].inputs);
-        }
-    }
-    ARMARX_VERBOSE << "PDO size: " << byteSum;
-    return retVal;
-}
-
-/**
- * Creates the Slave devices and adds them to the slaves list.
- * @see EtherCAT::slaves
- */
-std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr>> EtherCAT::createDevices()
-{
-    ARMARX_TRACE;
-    std::set<ControlDevicePtr> ctrlDevs;
-    std::set<SensorDevicePtr> sensDevs;
-    if (deviceContainerPtr == nullptr)
-    {
-        throw LocalException("no robot Container set! set a robotContainer before you start the bus!");
-    }
-    //dump infos about the device container
-    {
-        ARMARX_DEBUG << "device container:\n#functional devices"
-                     << deviceContainerPtr->getAllFunctionalDevices().size()
-                     << ARMARX_STREAM_PRINTER
-        {
-            const auto devs = deviceContainerPtr->getAllFunctionalDevices();
-            for (const auto& [idx, dev] : MakeIndexedContainer(devs))
-            {
-                out << "\n    idx " << idx
-                    << " initialized " << dev->isInitialized()
-                    << " class: " << dev->getClassName()
-                    << "\n        cfg nodes:";
-                for (const auto& c : dev->getNode().getChildPaths())
-                {
-                    out << "\n            " << c;
-                }
-            }
-        };
-    }
-
-    auto etherCATFactoryNames = EtherCATDeviceFactory::getAvailableClasses();
-    ARMARX_INFO << "etherCATFactoryNames: " << etherCATFactoryNames;
-    std::map<std::string, std::vector<uint16_t>> devicesOfType;
-    for (uint16_t currentSlaveIndex = 1; currentSlaveIndex <= ec_slavecount; currentSlaveIndex++)
-    {
-        ARMARX_TRACE;
-        const std::string messageSlaveIdentifier = "[Slave index: " + std::to_string(currentSlaveIndex) + "] ";
-        //DEBUG for EtherCAT HUB
-        //check the device type to identify the EtherCAT Hub
-        //        uint32 deviceType = ETHTERCAT_HUB_DEVICE_TYPE;
-        //uint32 vendorID = 0;
-        ARMARX_INFO << messageSlaveIdentifier << '\n'
-                    << "device type: " << std::hex << "0x" << ec_slave[currentSlaveIndex].Dtype   << std::dec << " (dec: " << ec_slave[currentSlaveIndex].Dtype   << ")\n"
-                    << "itype:       " << std::hex << "0x" << ec_slave[currentSlaveIndex].Itype   << std::dec << " (dec: " << ec_slave[currentSlaveIndex].Itype   << ")\n"
-                    << "eep_id:      " << std::hex << "0x" << ec_slave[currentSlaveIndex].eep_id  << std::dec << " (dec: " << ec_slave[currentSlaveIndex].eep_id  << ")\n"
-                    << "eep_man:     " << std::hex << "0x" << ec_slave[currentSlaveIndex].eep_man << std::dec << " (dec: " << ec_slave[currentSlaveIndex].eep_man << ")\n"
-                    << "eep_rev:     " << std::hex << "0x" << ec_slave[currentSlaveIndex].eep_rev << std::dec << " (dec: " << ec_slave[currentSlaveIndex].eep_rev << ")\n";
-        ARMARX_INFO << messageSlaveIdentifier << "CoE details " << (int)(ec_slave[currentSlaveIndex].mbx_proto & ECT_MBXPROT_COE);
-
-        bool foundDevice = false;
-        try
-        {
-            for (auto& facName : etherCATFactoryNames)
-            {
-                ARMARX_INFO << "Trying factory " << facName;
-                auto device = EtherCATDeviceFactory::fromName(facName, std::make_tuple(this, currentSlaveIndex, deviceContainerPtr));
-                ARMARX_INFO << "Device is " << device.get();
-                if (device)
-                {
-                    devicesOfType[device->getClassName()].emplace_back(currentSlaveIndex);
-
-                    auto newSlaves = device->getSlaves();
-                    ARMARX_INFO << "Found device of type: " << device->getClassName() << " with " << newSlaves.size() << " slaves";
-                    ARMARX_CHECK_GREATER(newSlaves.size(), 0);
-                    currentSlaveIndex += newSlaves.size() - 1;
-                    slaves.insert(slaves.end(), newSlaves.begin(), newSlaves.end());
-                    ctrlDevs.insert(device->getCtrlDevs().begin(), device->getCtrlDevs().end());
-                    sensDevs.insert(device->getSensDevs().begin(), device->getSensDevs().end());
-                    foundDevice = true;
-                    break;
-                }
-            }
-        }
-        catch (LocalException& e)
-        {
-            ARMARX_WARNING
-                    << "error during slave creation: " << messageSlaveIdentifier << e.getReason();
-            continue;
-        }
-        if (!foundDevice)
-        {
-            devicesOfType["<NO FACTORY FOUND>"].emplace_back(currentSlaveIndex);
-            ARMARX_ERROR << "Could not find a corresponding factory for " << messageSlaveIdentifier << " product id: " << (int)ec_slave[currentSlaveIndex].eep_id <<
-                         "vendor id: " << (int)ec_slave[currentSlaveIndex].eep_man;
-        }
-    }
-    ARMARX_INFO << "Summary of devices and factory types:\n" << ARMARX_STREAM_PRINTER
-    {
-        for (const auto& [factoryName, ids] : devicesOfType)
-        {
-            out << "---- " << factoryName << ": #" << ids.size() << " (ids:";
-            for (auto id : ids)
-            {
-                out << ' ' << id;
-            }
-            out << ")\n";
-        }
-    }
-            << '\n' << slaves.size() << " usable slave devices are initialized!" << std::endl;
-    return {std::vector<ControlDevicePtr>(ctrlDevs.begin(), ctrlDevs.end()), std::vector<SensorDevicePtr>(sensDevs.begin(), sensDevs.end())};
-}
-
-
-void EtherCAT::setSocketFileDescriptor(int socketFileDescriptor)
-{
-    this->socketFileDescriptor = socketFileDescriptor;
-}
-
-void EtherCAT::setIfName(const std::string& ifname)
-{
-    this->ifname = ifname;
-}
-
-/**
- * This updates all information on the bus, so all commands will be send to the Bus and all sensor and other monitored
- * values will be recived from the bus.
- * To run this the bus fist hast to be switched on, otherwise it will return false.
- * @see EtherCAT::switchBusOFF()
- * @see EtherCAT::switchBusON()
- * @return true if the loop was stop correct, false if there was an error or loop cloudn't even started.
- */
-bool EtherCAT::updateBus(bool doErrorHandling)
-{
-    ARMARX_TRACE;
-
-    if (!isSDOWorking)
-    {
-        ARMARX_INFO << "Control loop couldn't start - bus is not switched on\n";
-        return false;
-    }
-    else if (switchON_OFF == OFF)
-    {
-        if (!isSDOWorking)
-        {
-            ARMARX_WARNING << "Could not update bus because it switched off - closing bus (again?)";
-        }
-        closeBus();
-        return false;
-    }
-
-    if (switchON_OFF == ON)
-    {
-
-        // handle emergency stop release
-        bool emergencyStopActive = isEmergencyStopActive();
-        auto now = IceUtil::Time::now();
-        auto recoverTriggerAge = (now - ermergencyStopRecoverStartpoint);
-
-        // the elmo sometimes go from emergency stop state back into fault state for a moment - the time variable is the fix for that
-        if ((!emergencyStopActive && emergencyStopWasActivated) ||  recoverTriggerAge.toSecondsDouble() < 2.0)
-        {
-            if (recoverTriggerAge.toSecondsDouble() > 2)
-            {
-                ermergencyStopRecoverStartpoint = now;
-            }
-            bool recovered = true;
-            for (AbstractSlavePtr& slave : slaves)
-            {
-                recovered &= slave->recoverFromEmergencyStop();
-            }
-            if (recovered)
-            {
-                ARMARX_RT_LOGF_IMPORTANT("Recovered!");
-                emergencyStopWasActivated = false;
-            }
-        }
-        else if (emergencyStopActive && !emergencyStopWasActivated)
-        {
-            ARMARX_RT_LOGF_IMPORTANT("EMERGENCY STOP ACTIVE");
-            emergencyStopWasActivated = emergencyStopActive;
-        }
-
-
-
-        //execute every slave
-        for (AbstractSlavePtr& slave : slaves)
-        {
-
-            //try to clear error if there exist some, the rest of the slaves can run normal
-            if (!emergencyStopActive && slave->hasError())
-            {
-                //if the errors can't be fixed we will switch of the bus with an error
-                if (doErrorHandling && !slave->handleErrors())
-                {
-                    error = true;
-                }
-                //ARMARX_WARNING << "slave: " << slave->getSlaveNumber() << " had error";
-            }
-            else
-            {
-                slave->execute();
-            }
-        }
-
-        auto delay = (IceUtil::Time::now(IceUtil::Time::Monotonic) - busUpdateLastUpdateTime);
-        if (delay.toMilliSecondsDouble() > 40)
-        {
-            ARMARX_RT_LOGF_WARN("Update bus was not called for a long time: %d ms", delay.toMilliSecondsDouble()).deactivateSpam(5);
-        }
-
-        //send an receive process data
-        //        RT_TIMING_START(UpdatePDO);
-        updatePDO();
-        //        RT_TIMING_CEND(UpdatePDO, 0.7);
-        busUpdateLastUpdateTime = IceUtil::Time::now(IceUtil::Time::Monotonic);
-
-
-        //error handling
-        if (doErrorHandling)
-        {
-            errorHandling();
-        }
-    }
-
-    return true;
-}
-
-/**
- * This sets the flag to switch off the bus.
- * If the bus is already set to switch off this will has no effect.
- * @see EtherCAT::closeBus()
- */
-void EtherCAT::switchBusOFF()
-{
-    if (switchON_OFF == OFF)
-    {
-        ARMARX_VERBOSE << deactivateSpam(1) << "Bus is already switched off";
-        return;
-    }
-    readErrorCounters();
-    ARMARX_INFO << "Switching off bus";
-    switchON_OFF = OFF;
-
-}
-
-bool EtherCAT::isBusInOperationalMode()
-{
-    return busInOperationalMode;
-}
-
-/**
- * This closes the Bus
- */
-void EtherCAT::closeBus()
-{
-    ARMARX_INFO << "Bus is shutting down";
-
-
-    //from now we doing all we can to be not in Op mode and we don't want anybody to send PDO's anymore
-    busInOperationalMode = false;
-
-
-    //shutdown the slaves if the bus hasn't died
-    if (!isBusDead && !error)
-    {
-        bool found;
-        do
-        {
-            found = false;
-            for (auto slave : slaves)
-            {
-                ARMARX_INFO << deactivateSpam(1) << "shutting down slave " << slave->getSlaveIdentifier().humanName << " (" << slave->getSlaveNumber() << "/" << slaves.size() << ")" << std::endl;
-                found |= !slave->shutdown();
-                //            {
-                //                if((std::chrono::duration_cast<std::chrono::seconds>(
-                //                        std::chrono::high_resolution_clock::now() - startCycle).count() < 5))
-                //                {
-
-                //                }
-                //            }
-            }
-            updatePDO();
-        }
-        while (found);
-    }
-
-    //indicate that bus is closed
-    isSDOWorking = false;
-    pdoMapped = false;
-
-    //shutting down  bus
-    ///requesting init for all slaves
-    ec_slave[0].state = EC_STATE_INIT;
-    ec_writestate(0);
-
-    ///closing bus
-    ec_close();
-
-    ARMARX_INFO << "Bus closed" << std::endl;
-}
-
-/**
- * This deactivates the Complete access mode in CoE for the given slave.
- * For Elmo's it is necessary to deactivate the CA mode otherwise soem isn't able to bring them into OP-Mode
- * @param slave the slave for which the CA mode will be deactivated
- */
-void EtherCAT::deactivateCOECA(AbstractSlave* slave)
-{
-    ARMARX_INFO << "Deactivation CoE Complete Access for slave:" << slave->getSlaveNumber()
-                << std::endl;
-    uint8 config = ec_slave[slave->getSlaveNumber()].CoEdetails;
-    config &= ~ECT_COEDET_SDOCA;
-    ec_slave[slave->getSlaveNumber()].CoEdetails = config;
-}
-
-/**
- * If there is a erro on the bus this prints the AL Status code:
- */
-void EtherCAT::printALStatusError(uint16_t slave)
-{
-    std::string name = "Unknown";
-
-    AbstractSlavePtr slavePtr =  getSlaveAtIndex(slave);
-    if (slavePtr)
-    {
-        name = slavePtr->getSlaveIdentifier().humanName;
-    }
-
-    ARMARX_ERROR << "Slave " << name << " (number: " << slave << ") State=0x" << std::hex << EtherCATStateToString(static_cast<ec_state>(ec_slave[slave].state))
-                 << " StatusCode=0x" << ec_slave[slave].ALstatuscode  << " : "
-                 << ec_ALstatuscode2string(ec_slave[slave].ALstatuscode)
-                 << ", name: " << ec_slave[slave].name;
-}
-
-int EtherCAT::ecx_APRD_with_error_handling(uint16_t ADP, uint16_t ADO, uint16_t length, void* data, int timeout, uint16_t slaveIndex, const std::string& name, int port)
-{
-    int retval = ecx_APRD(ecx_context.port, ADP, ADO, length, data, timeout);
-    if (retval <= 0)
-    {
-        std::stringstream ss;
-        ss << "0x" << std::hex << std::setw(4) << std::setfill('0') << ADO;
-        ARMARX_ERROR << "ecx_APRD failed: WC = " << retval << " (-1: EC_NOFRAME) . Slavenumber " << slaveIndex << " port:" << port << "\tname: " << name
-                     << "\taddr: " << ss.str();
-    }
-    return retval;
-}
-
-void EtherCAT::readErrorCounters()
-{
-    IceUtil::Time start = IceUtil::Time::now(IceUtil::Time::Monotonic);
-    for (uint16 slaveIndex = 1; slaveIndex <= *(ecx_context.slavecount); slaveIndex++)
-    {
-        std::string name = "Unknown";
-
-        AbstractSlavePtr slavePtr =  getSlaveAtIndex(slaveIndex);
-        if (slavePtr)
-        {
-            name = slavePtr->getSlaveIdentifier().humanName;
-        }
-
-        uint16 ADPh = (uint16)(1 - slaveIndex);
-
-        //not used, only confusing info...
-        //uint16_t configAddr = ecx_APRDw(ecx_context.port, ADPh, ECT_REG_STADR, 100000);
-        for (int i = 0; i < 2; i++)
-        {
-
-            // Error handling taken from
-            // ethercat_esc_datasheet_sec1_technology_2i2: Chapter 14 Error counters, Table 50:
-            // see: RobotAPI/etc/doc/ethercat/ethercat_esc_datasheet_sec1_technology_2i2.pdf
-            // or https://www.ethercat.org/download/documents/EtherCAT_Diagnosis_For_Users_DE.pdf
-            // or RobotAPI/etc/doc/EtherCAT_Diagnosis_For_Users_DE.pdf
-            uint8_t rxErrorCounter = 0;
-            uint8_t invalidFrameCounter = 0; // or Physical error count
-            uint8_t forwardedRxErrorCounter = 0; // shows the error counter of a predecessor
-            uint8_t linkLostCounter = 0;
-            int ret1 = ecx_APRD_with_error_handling(ADPh, 0x300 + i * 2, 1, &invalidFrameCounter, 100000, slaveIndex, name, i);
-            int ret2 = ecx_APRD_with_error_handling(ADPh, 0x301 + i * 2, 1, &rxErrorCounter, 100000, slaveIndex, name, i);
-            int ret3 = ecx_APRD_with_error_handling(ADPh, 0x308 + i, 1, &forwardedRxErrorCounter, 100000, slaveIndex, name, i);
-            int ret4 = ecx_APRD_with_error_handling(ADPh, 0x310 + i, 1, &linkLostCounter, 100000, slaveIndex, name, i);
-
-            if (rxErrorCounter > 0 || invalidFrameCounter > 0 || forwardedRxErrorCounter > 0 || linkLostCounter > 0)
-            {
-                ARMARX_ERROR << "Errors found for Slavenumber " << slaveIndex << " port:" << i << "\tname: " << name << ": "
-                             << VAROUT((int)rxErrorCounter) << "\t" << VAROUT((int)invalidFrameCounter) << "\t" << VAROUT((int)forwardedRxErrorCounter) << "\t" << VAROUT((int)linkLostCounter);
-            }
-            else if (ret1 > 0 && ret2 > 0 && ret3 > 0 && ret4 > 0)
-            {
-                ARMARX_DEBUG << "no errors for Slavenumber " << slaveIndex << " port:" << i << "\tname: " << name;
-            }
-        }
-        IceUtil::Time elapsed = (IceUtil::Time::now(IceUtil::Time::Monotonic) - start);
-        if (elapsed.toMilliSeconds() > 10)
-        {
-            updatePDO();
-            ARMARX_DEBUG << "Updated BUS to prevent timeout, " << elapsed << " has passed since last bus update.";
-            start = IceUtil::Time::now(IceUtil::Time::Monotonic);
-        }
-    }
-}
-
-void EtherCAT::errorHandling()
-{
-    uint16 slave;
-    //if the bus is in already in safe op then we have an error and don't need any more stuff to do
-    error = ec_slave[0].state == EC_STATE_SAFE_OP;
-
-    if (lastWorkCounter == expectedWKC)
-    {
-        noErrorIterations++;
-    }
-    ARMARX_ON_SCOPE_EXIT
-    {
-        firstErrorCheck = false;
-    };
-    if (((lastWorkCounter < expectedWKC) || ec_group[currentGroup].docheckstate) && !error)
-    {
-
-
-        ARMARX_RT_LOGF_WARN("RECOVERY - Wkc: %s - %d/%d, state: %s - iterations without error: %d", ((lastWorkCounter < expectedWKC) ? "NOT OK" : "OK"),
-                            lastWorkCounter, expectedWKC, (ec_group[currentGroup].docheckstate ? "NOT OK" : "OK"), noErrorIterations);
-
-        //actually there is something wrong so we have an error lets see if we can find an fix it
-        error = true;
-        if (checkErrorCountersOnWKCError /* || (!firstErrorCheck && noErrorIterations == 0)*/)
-        {
-            readErrorCounters();
-        }
-        noErrorIterations = 0;
-
-        /* one ore more slaves are not responding */
-        //This is hard SOEM code not the easy stuff, but works...
-        ec_group[currentGroup].docheckstate = FALSE;
-        ec_readstate();
-        for (slave = 1; slave <= ec_slavecount; slave++)
-        {
-            if ((ec_slave[slave].group == currentGroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
-            {
-                ec_group[currentGroup].docheckstate = TRUE;
-                if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
-                {
-                    ARMARX_RT_LOGF_WARN("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.", slave);
-                    //Reading as much data form the slave we can
-                    //AL Status code (EtherCAT)
-                    ARMARX_RT_LOGF_WARN("EtherCAT::errorHandling - AbstractSlave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
-                                        slave, ec_slave[slave].state, ec_slave[slave].ALstatuscode,
-                                        ec_ALstatuscode2string(ec_slave[slave].ALstatuscode));
-                    //Abort Code (Coe - Ds 301)
-                    ARMARX_RT_LOGF_WARN("Error: %s \n", ec_elist2string());
-
-
-                    ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
-                    ec_writestate(slave);
-                    //there is a chance to recover
-                    error = false;
-                }
-                else if (ec_slave[slave].state == EC_STATE_SAFE_OP)
-                {
-                    ARMARX_RT_LOGF_WARN("slave %d is in SAFE_OP, change to OPERATIONAL.", slave);
-                    ec_slave[slave].state = EC_STATE_OPERATIONAL;
-                    ec_writestate(slave);
-                    //we recovered
-                    error = false;
-                }
-                else if (ec_slave[slave].state > 0)
-                {
-                    ARMARX_RT_LOGF_WARN("slave %d has a bad state", slave);
-                    if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
-                    {
-                        ec_slave[slave].islost = FALSE;
-                        ARMARX_RT_LOGF_WARN("slave %d reconfigured", slave);
-                        //we recovered
-                        error = false;
-                    }
-                }
-                else if (!ec_slave[slave].islost)
-                {
-                    /* re-check state */
-                    ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
-                    if (!ec_slave[slave].state)
-                    {
-                        ec_slave[slave].islost = TRUE;
-                        ARMARX_ERROR << "ERROR : slave " << slave << " lost";
-                    }
-                }
-            }
-            //ARMARX_IMPORTANT << __LINE__ << " before is Lost " << slave << " " << (ec_slave[slave].islost ? "TRUE" : "False") ;
-            if (ec_slave[slave].islost)
-            {
-                if (!ec_slave[slave].state)
-                {
-                    if (ec_recover_slave(slave, EC_TIMEOUTMON))
-                    {
-                        ec_slave[slave].islost = FALSE;
-                        ARMARX_RT_LOGF_WARN("slave %d recovered", slave);
-                        //we recovered
-                        error = false;
-                    }
-                    //we couldn't recover the slave so it is lost
-                    ARMARX_RT_LOGF_ERROR("Could not recover slave %d", slave);
-                    ec_slave[slave].islost = TRUE;
-                    error = true;
-                    continue;
-                }
-                else
-                {
-                    ec_slave[slave].islost = FALSE;
-                    ARMARX_RT_LOGF_WARN("slave %d found", slave);
-                }
-            }
-        }
-        if (!ec_group[currentGroup].docheckstate)
-        {
-            ARMARX_RT_LOGF_WARN("all slaves resumed into OPERATIONAL-Mode");
-            error = false;
-        }
-        else
-        {
-            ARMARX_ERROR << "Bus is not ok! ";
-            readErrorCounters();
-            //if we get here we have an error but no solution to fix this, very sad
-            error = true;
-        }
-    }
-
-    //if there is an error and we don't already switched to safe op we can skipp this.
-    if (error && ec_slave[0].state != EC_STATE_SAFE_OP)
-    {
-        ARMARX_ERROR << "Bus detected an Error, maybe one slave or the whole bus died! - Switching in Safe-Op Modus "
-                     << "No Targets will be accepted bei the slaves anymore";
-        //switching to safe op so we receive data but the slaves won't accept any new targets
-        ec_slave[0].state = EC_STATE_SAFE_OP;
-        ec_writestate(0);
-    }
-
-    // check SOEM error list(e.g. from mailbox) for errors
-    if (ec_iserror())
-    {
-        ec_errort error;
-        while (ec_poperror(&error))
-        {
-            auto slave = getSlaveAtIndex(error.Slave);
-            if (error.Etype == EC_ERR_TYPE_EMERGENCY)
-            {
-                ARMARX_RT_LOGF_WARN("Found emergency error for slave %s (id: %d) from timestamp %d: error code: %d, error reg: %d, error index: %d, error sub index: %d",
-                                    (slave ? slave->getSlaveIdentifier().humanName.c_str() : "unknown"),
-                                    error.Slave, error.Time.sec, error.ErrorCode, error.ErrorReg, error.Index, error.SubIdx);
-            }
-            else
-            {
-                ARMARX_RT_LOGF_WARN("Found error for slave %s (id: %d) from timestamp %d: error type: %s, error index: %d, error sub index: %d",
-                                    (slave ? slave->getSlaveIdentifier().humanName.c_str() : "unknown"),
-                                    error.Slave, error.Time.sec, EC_ErrorTypeToString(error.Etype), error.Index, error.SubIdx);
-            }
-        }
-    }
-}
-
-std::vector<SensorDevicePtr> EtherCAT::getSensDevs() const
-{
-    return sensDevs;
-}
-
-std::vector<ControlDevicePtr> EtherCAT::getCtrlDevs() const
-{
-    return ctrlDevs;
-}
-
-bool EtherCAT::rebootBus()
-{
-    isSDOWorking = false;
-    ARMARX_IMPORTANT << "Current bus state: " << EC_StateToString(ec_slave[0].state);
-    if (ec_slave[0].state == EC_STATE_INIT)
-    {
-        return startBus(false);
-    }
-    return true;
-}
-
-const char* EtherCAT::EC_StateToString(uint16 state)
-{
-    switch (state)
-    {
-        case EC_STATE_NONE:
-            return "EC_STATE_NONE";
-        case EC_STATE_INIT:
-            return "EC_STATE_INIT";
-        case EC_STATE_PRE_OP:
-            return "EC_STATE_PRE_OP";
-        case EC_STATE_BOOT:
-            return "EC_STATE_BOOT";
-        case EC_STATE_SAFE_OP:
-            return "EC_STATE_SAFE_OP";
-        case EC_STATE_OPERATIONAL:
-            return "EC_STATE_OPERATIONAL";
-        case EC_STATE_ACK:
-            return "EC_STATE_ACK";
-    }
-    throw std::logic_error {__FILE__ ": " + to_string(__LINE__) + ": unhandled state: " + to_string(state)};
-}
-
-const char* EtherCAT::EC_ErrorTypeToString(uint16 errorType)
-{
-    switch (errorType)
-    {
-        case EC_ERR_TYPE_SDO_ERROR:
-            return "EC_ERR_TYPE_SDO_ERROR";
-        case EC_ERR_TYPE_EMERGENCY:
-            return "EC_ERR_TYPE_EMERGENCY";
-        case EC_ERR_TYPE_PACKET_ERROR:
-            return "EC_ERR_TYPE_PACKET_ERROR";
-        case EC_ERR_TYPE_SDOINFO_ERROR:
-            return "EC_ERR_TYPE_SDOINFO_ERROR";
-        case EC_ERR_TYPE_FOE_ERROR:
-            return "EC_ERR_TYPE_FOE_ERROR";
-        case EC_ERR_TYPE_FOE_BUF2SMALL:
-            return "EC_ERR_TYPE_FOE_BUF2SMALL";
-        case EC_ERR_TYPE_FOE_PACKETNUMBER:
-            return "EC_ERR_TYPE_FOE_PACKETNUMBER";
-        case EC_ERR_TYPE_SOE_ERROR:
-            return "EC_ERR_TYPE_SOE_ERROR";
-        case EC_ERR_TYPE_MBX_ERROR:
-            return "EC_ERR_TYPE_MBX_ERROR";
-        case EC_ERR_TYPE_FOE_FILE_NOTFOUND:
-            return "EC_ERR_TYPE_FOE_FILE_NOTFOUND";
-    }
-    throw std::logic_error {__FILE__ ": " + to_string(__LINE__) + ": unhandled error type: " + to_string(errorType)};
-}
-
-
-
-
-const std::atomic_bool& EtherCAT::getIsBusDead() const
-{
-    return isBusDead;
-}
-
-/**
- * Uperror the PDO and updates the lastWorkingCounter to latest receive event
- * @see EtherCAT::lastWorkingCounter
- */
-void EtherCAT::updatePDO()
-{
-
-    static int count = 0;
-    static long tx_max = 0;
-    static long rx_max = 0;
-
-
-    auto PDO_Tx = IceUtil::Time::now();
-    auto PDO_Rx = IceUtil::Time::now();
-    //TIMING_START(PDO_Tx);
-    ec_send_processdata();
-    //TIMING_END(PDO_Tx);
-    long tx_elapsed = (IceUtil::Time::now() - PDO_Tx).toMicroSeconds();
-
-
-    //TIMING_START(PDO_Rx);
-    lastWorkCounter = ec_receive_processdata(EC_TIMEOUTMON * 10);
-    //TIMING_END(PDO_Rx);
-    long rx_elapsed = (IceUtil::Time::now() - PDO_Rx).toMicroSeconds();
-
-    tx_max = std::max(tx_max, tx_elapsed);
-    rx_max = std::max(rx_max, rx_elapsed);
-
-    count++;
-    if (count >= 1000 && false)
-    {
-        count = 0;
-        ARMARX_RT_LOGF_INFO("TX max: %d, µs, RX max: %d µs", tx_max, rx_max);
-
-        tx_max = 0;
-        rx_max = 0;
-    }
-}
-
-/**
- * Returns all identifiied slaves on the bus
- * @return
- */
-std::vector<AbstractSlavePtr> EtherCAT::getSlaves()
-{
-    return slaves;
-}
-
-EtherCAT::~EtherCAT()
-{
-
-}
-
-/**
- * This starts the bus.
- * If the is already started then nothing will be done.
- * @see EtherCAT::startBus()
- */
-bool EtherCAT::switchBusON()
-{
-    //check if the bus is already running
-    if (switchON_OFF == ON)
-    {
-        return true;
-    }
-    //otherwise start it
-    switchON_OFF = ON;
-    return startBus(true);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeByteBuffer(unsigned char* buf, int buflen, uint16_t slave, uint16_t index, uint8_t subindex,
-                               bool CompleteAccess)
-{
-    if (!busShouldBeRunning())
-    {
-        return false;
-    }
-    int workCount;
-    workCount = ec_SDOwrite(slave, index, subindex, (boolean) CompleteAccess, buflen, buf, 5000);
-    ARMARX_DEBUG << "Writing Buffer to slave: " << slave << " index 0x" << std::hex << index << std::dec << ":" << subindex << ", wc " << workCount << ": " << (workCount > 0 ? "success" : " failure");
-    if (workCount > 0)
-    {
-        return true;
-    }
-    else
-    {
-        return false;
-    }
-}
-
-bool EtherCAT::readByteBuffer(uint16_t slave, uint16_t index, uint8_t subindex, unsigned char* buf, int buflen,
-                              bool CompleteAccess)
-{
-    return generalSDORead(slave, index, subindex, buflen, buf, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, int8_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
-    int buflen = 1;
-    return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, uint8_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
-    int buflen = 1;
-    return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, uint16_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
-    int buflen = 2;
-    return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, uint32_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
-    int buflen = 4;
-    return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, int32_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
-    int buflen = 4;
-    return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint8_t& value) const
-{
-    int buflen = 1;
-    return generalSDORead(slave, index, subindex, buflen, &value);
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int8_t& value) const
-{
-    int buflen = 1;
-    return generalSDORead(slave, index, subindex, buflen, &value);
-
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint16_t& value, bool CompleteAccess) const
-{
-    int buflen = 2;
-    return generalSDORead(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int16_t& value) const
-{
-    int buflen = 2;
-    return generalSDORead(slave, index, subindex, buflen, &value);
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int32_t& value) const
-{
-    int buflen = 4;
-    return generalSDORead(slave, index, subindex, buflen, &value);
-
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint32_t& value) const
-{
-    int buflen = 4;
-    return generalSDORead(slave, index, subindex, buflen, &value);
-}
-
-/**
- * This will return the Vendor ID of the slave with the given slave number
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave number of the requested slave
- * @param [OUT] vendorID
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::getVendorID(uint16_t slave, uint32_t& vendorID) const
-{
-    //    uint32 serial;
-    bool retVal = readSDO(slave, 0x1018, 1, vendorID);
-    //    getSerialNumber(slave, serial);
-    ARMARX_INFO << "Vendor ID of slave " << slave << " 0x" << std::hex << vendorID << std::dec << " (" << vendorID
-                << ")";
-    //                << " serial number: " << serial;
-    //    if (retVal && (vendorID == ELMO_VENDOR_ID))
-    //    {
-    //        ARMARX_DEBUG << "(Elmomc)";
-    //    }
-    //    else if (retVal && (vendorID == H2T_VENDOR_ID))
-    //    {
-    //        ARMARX_DEBUG << "(H2T)";
-    //    }
-    //    else if (retVal)
-    //    {
-    //        ARMARX_WARNING << "Unknown vendor";
-    //    }
-    //    else
-    //    {
-    //        ARMARX_ERROR << "reading Vendor Id failed";
-    //    }
-    return retVal;
-}
-
-
-bool EtherCAT::getSerialNumber(uint16 slave, uint32& serialNumber) const
-{
-    bool retVal = readSDO(slave, 0x1018, 4, serialNumber);
-    return retVal;
-}
-
-
-bool EtherCAT::getProductCode(uint16_t slave, uint32_t& productCode) const
-{
-    bool retVal = readSDO(slave, 0x1018, 2, productCode);
-    //printf("Product Code of slave %d: 0x%x (%d)", slave, productCode, productCode);
-    ARMARX_DEBUG << "Product Code of slave " << slave << ": 0x" << std::hex << productCode << std::dec
-                 << " (" << productCode << ")";
-    //    if (retVal && (productCode == ELMO_ACTOR_PRODUCT_CODE))
-    //    {
-    //        ARMARX_DEBUG << "actor elmo";
-    //    }
-    //    else if (retVal && (productCode == H2T_SENSOBOARD_PRODUCT_CODE))
-    //    {
-    //        ARMARX_DEBUG << "sensor board";
-    //    }
-    //    else if (retVal)
-    //    {
-    //        ARMARX_WARNING << "Unknown product code ";
-    //    }
-    //    else
-    //    {
-    //        ARMARX_ERROR << "reading Product Code failed";
-    //    }
-    return retVal;
-}
-
-/**
- * This is the private genearl SDO write function which doesn't do any typ checks to the given write value.
- * It checks if the SDO write was successful an prints a message
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave
- * @param [IN] index
- * @param [IN] subindex
- * @param [IN] buflen
- * @param [IN] buf
- * @param [IN] ca
- * @return
- */
-bool EtherCAT::generalSDOWrite(uint16_t slave, uint16_t index, uint8_t subindex, int buflen, void* buf, bool ca)
-{
-    if (!busShouldBeRunning())
-    {
-        ARMARX_WARNING << "Bus is not running no write";
-        return false;
-    }
-    std::unique_lock lock(etherCatMutex);
-    IceUtil::Time start = IceUtil::Time::now();
-    int workCount = ec_SDOwrite(slave, index, subindex, (boolean) ca, buflen, buf, SDO_WRITE_TIMEOUT);
-    IceUtil::Time end = IceUtil::Time::now();
-    //printf("EtherCAT::generalSDOWrite - Writing 0x%x (%d) to slave: %d index 0x%x:%d : ", *((int*) buf),
-    //       *((int*) buf), slave, index, subindex);
-    /*ARMARX_INFO << "Writing 0x" << std::hex << std::uppercase << *((int*) buf) << std::dec << "(" << *((int*) buf) << ")"
-                << " to slave: " << slave << " index 0x" << std::hex << std::uppercase << index << std::dec << ":"
-                << (int32) subindex;*/
-    if (workCount > 0)
-    {
-        //ARMARX_INFO << "success.\n";
-        return true;
-    }
-    else
-    {
-        ARMARX_WARNING << std::hex << "Error while writing at 0x" << index << ":" << (int)subindex;
-        ARMARX_WARNING << "failure. work counter: " << workCount << " writing took " << (end - start).toMicroSeconds() << std::endl;
-        printALStatusError(slave);
-        //Read all the errors
-        ARMARX_WARNING << ec_elist2string();
-        return false;
-    }
-}
-
-/**
- * This is the private genearl SDO write function which doesn't do any typ checks to the given write value.
- * It checks if the SDO write was successful an prints a message.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave
- * @param [IN] index
- * @param [IN] subindex
- * @param [IN] buflen
- * @param [OUT] buf
- * @param [IN] ca complete access by default false
- * @return
- */
-bool EtherCAT::generalSDORead(uint16_t slave, uint16_t index, uint8_t subindex, int buflen, void* buf, bool ca) const
-{
-    if (!busShouldBeRunning())
-    {
-        return false;
-    }
-    std::unique_lock lock(etherCatMutex);
-    IceUtil::Time start = IceUtil::Time::now();
-    int workCount = ec_SDOread(slave, index, subindex, (boolean) ca, &buflen, buf, SDO_READ_TIMEOUT);
-    IceUtil::Time end = IceUtil::Time::now();
-    if (workCount > 0)
-    {
-        //printf("EtherCAT::generalSDORead- Reading 0x%x (%d), from slave: %d index 0x%x:%d\n", *((uint16*) buf),
-        //       *((uint16*) buf), slave, index,
-        //       subindex);
-        /*ARMARX_INFO << "Reading 0x" << std::hex << std::uppercase << *((uint16*) buf) << std::dec << "(" << *((uint16*) buf) << ")"
-                    << " from slave: " << slave << " index 0x" << std::hex << std::uppercase << index << std::dec << ":"
-                    << (int32) subindex;*/
-        return true;
-    }
-    else
-    {
-        ARMARX_WARNING << std::hex << "Error while reading index 0x" << index << ":" << (int)subindex
-                       << " of slave " << slave << " into a buffer of len " << buflen
-                       << " ca = " << ca;
-        ARMARX_WARNING << "work counter (has to be >0): " << workCount << " reading took " << (end - start).toMicroSeconds() << " µs - error message from SOEM: " << std::string(ec_elist2string());
-        return false;
-    }
-}
-
-void* EtherCAT::getIOMap() const
-{
-    return static_cast<void*>(ec_slave[0].outputs);
-}
-
-int EtherCAT::getMappedSize()
-{
-    return actualMappedSize;
-}
-
-AbstractSlavePtr EtherCAT::getSlaveAtIndex(uint16_t slaveIndex) const
-{
-    for (AbstractSlavePtr slave : slaves)
-    {
-        //        ARMARX_INFO << "Checking slave: " << slave->getSlaveNumber() << " vs  " << slaveId;
-        if (slave->getSlaveNumber() == slaveIndex)
-        {
-            return slave;
-        }
-    }
-    return AbstractSlavePtr();
-}
-
-std::array<char, IOmapSize>& EtherCAT::getIOMapBuffer()
-{
-    return IOmap;
-}
-
-const std::atomic_bool& EtherCAT::getError() const
-{
-    return error;
-}
-
-bool EtherCAT::isPDOMapped() const
-{
-    return pdoMapped;
-}
-
-void EtherCAT::setDeviceContainerPtr(const DeviceContainerPtr& deviceContainerPtr)
-{
-    EtherCAT::deviceContainerPtr = deviceContainerPtr;
-}
-
-void EtherCAT::setMainUnitPtr(RobotUnit* mainUnitPtr)
-{
-    EtherCAT::mainUnitPtr = mainUnitPtr;
-}
-
-std::string EtherCAT::EtherCATStateToString(u_int16_t state)
-{
-    switch (state)
-    {
-        case EC_STATE_NONE:
-            return "EC_STATE_NONE";
-        case EC_STATE_INIT:
-            return "EC_STATE_INIT";
-        case EC_STATE_PRE_OP:
-            return "EC_STATE_PRE_OP";
-        case EC_STATE_BOOT:
-            return "EC_STATE_BOOT";
-        case EC_STATE_SAFE_OP:
-            return "EC_STATE_SAFE_OP";
-        case EC_STATE_OPERATIONAL:
-            return "EC_STATE_OPERATIONAL";
-        case EC_STATE_ERROR:
-            return "EC_STATE_ERROR or EC_STATE_ACK";
-    }
-    return "UNKNOWN_STATE";
-}
-
-bool EtherCAT::isEmergencyStopActive() const
-{
-    bool found = false;
-    for (const AbstractSlavePtr& slave : this->slaves)
-    {
-        if (slave->isEmergencyStopActive())
-        {
-            found = true; // dont break so that isEmergencyStopActive executed for each slave
-        }
-    }
-    return found;
-}
-
-
-bool EtherCAT::getCheckErrorCountersOnWKCError() const
-{
-    return checkErrorCountersOnWKCError;
-}
-
-void EtherCAT::setCheckErrorCountersOnWKCError(bool value)
-{
-    checkErrorCountersOnWKCError = value;
-}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
deleted file mode 100644
index f103df995c10cb2210071b2b58c8a368231d06d9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
+++ /dev/null
@@ -1,249 +0,0 @@
-#pragma once
-#include "ArmarXEtherCATFwd.h"
-
-#include "AbstractFunctionalDevice.h"
-
-#include <IceUtil/Time.h>
-
-#include <iostream>
-#include <fstream>
-#include <vector>
-#include <memory>
-#include <sstream>
-#include <atomic>
-#include <mutex>
-
-
-/**
- * The actual size of the mapped prozess image will be smaller but with 4096 we are quite
- * confident that we will have enough space
- */
-#define IOmapSize 4096
-
-/**  master -> slave */
-#define RX_MAPPING_INDEX 0x1C12
-/**  slave -> master */
-#define TX_MAPPING_INDEX 0x1C13
-
-/** The ESC of the EtherCAT Hub just give a device type not much more so we identify them via der device type an ingnore them */
-#define ETHTERCAT_HUB_DEVICE_TYPE 0x1
-
-/// This defenitions are to make the switching on and off of the bus clear
-#define ON true
-#define OFF false
-
-//DEBUG
-//#define RTTIME_TEST
-
-
-namespace armarx
-{
-    using ControlDevicePtr = std::shared_ptr<class ControlDevice>;
-    using SensorDevicePtr = std::shared_ptr<class SensorDevice>;
-
-
-    class RobotUnit;
-    class EtherCAT
-    {
-    public:
-        static EtherCAT& getBus();
-
-        void setDeviceContainerPtr(const DeviceContainerPtr& deviceContainerPtr);
-
-        void setMainUnitPtr(RobotUnit* mainUnitPtr);
-
-        void setSocketFileDescriptor(int socketFileDescriptor);
-
-        void setIfName(const std::string& ifname);
-
-        bool getVendorID(uint16_t slave, uint32_t& vendorID) const;
-
-        /**
-         * With this method one can check if the bus is started to operational mode.
-         * This means you can use PDO's if bus is not in Operational mode pdo's not avaliable and can cause segmentation faults
-         * @return true if control loop runs
-         */
-        bool isBusInOperationalMode();
-
-        /**
-         * With this method on can check if bus hat already mapped the PDO
-         * @return
-         */
-        bool isPDOMapped() const;
-
-        bool updateBus(bool doErrorHandling = true);
-
-        void switchBusOFF();
-
-        bool switchBusON();
-
-        void deactivateCOECA(AbstractSlave* slave);
-
-        void* getIOMap() const;
-        std::array<char, IOmapSize>& getIOMapBuffer();
-
-        int getMappedSize();
-        AbstractSlavePtr getSlaveAtIndex(uint16_t slaveIndex) const;
-        std::vector<AbstractSlavePtr> getSlaves();
-
-        bool getProductCode(uint16_t slave, uint32_t& productCode) const;
-
-        bool getSerialNumber(uint16_t slave, uint32_t& serialNumber) const;
-
-        /**
-         * This indicates if there is an error on the bus, maybe a slaved died or someting else, most times it
-         * would be the best way to shut down the robot. But this is only an error, we are still able to communicate with
-         * the bus. For a complete fail there is the indication isBusDead
-         * @see EtherCAT::getIsBusDead()
-         * @return TURE if there is a error on the bus
-         */
-        const std::atomic_bool& getError() const;
-
-        /**
-         * This indicates if the bus is completly dead, may power was switched off.
-         * For just getting errors on the bus ther is the indication error
-         * @see EtherCAT::getError()
-         * @return TRUE if the bus is noch reachable anymore
-         */
-        const std::atomic_bool& getIsBusDead() const;
-
-        //read and writes
-        bool writeByteBuffer(unsigned char* buf, int buflen, uint16_t slave, uint16_t index, uint8_t subindex,
-                             bool CompleteAccess = false);
-
-        bool writeSDO(uint16_t slave, int8_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
-        bool writeSDO(uint16_t slave, uint8_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
-        bool writeSDO(uint16_t slave, uint16_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
-        bool writeSDO(uint16_t slave, uint32_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
-        bool writeSDO(uint16_t slave, int32_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
-        bool readByteBuffer(uint16_t slave, uint16_t index, uint8_t subindex, unsigned  char* buf, int buflen, bool CompleteAccess = false);
-
-        bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint8_t& value) const;
-
-        bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int8_t& value) const;
-
-        bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint16_t& value, bool CompleteAccess = false) const;
-
-        bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int16_t& value) const;
-
-        bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int32_t& value) const;
-
-        bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint32_t& value) const;
-
-        bool goToSafeOp();
-        static std::string EtherCATStateToString(uint16_t state);
-        bool isEmergencyStopActive() const;
-
-        bool rebootBus();
-
-        static const char* EC_StateToString(uint16_t state);
-        static const char* EC_ErrorTypeToString(uint16_t errorType);
-
-        std::vector<ControlDevicePtr> getCtrlDevs() const;
-
-        std::vector<SensorDevicePtr> getSensDevs() const;
-
-        bool getCheckErrorCountersOnWKCError() const;
-        void setCheckErrorCountersOnWKCError(bool value);
-
-        void readErrorCounters();
-    protected:
-
-        int ecx_APRD_with_error_handling(uint16_t ADP, uint16_t ADO, uint16_t length, void* data, int timeout, uint16_t slaveIndex, const std::string& name, int port);
-    private:
-        //Hiding the constructor to avoid illegal creation of the Bus (singelton pattern)
-        EtherCAT();
-        ~EtherCAT();
-
-        //avoid coping the object (singelton pattern)
-        EtherCAT(const EtherCAT&);
-        EtherCAT& operator=(const EtherCAT&);
-
-
-        //starting and closing the bus should only be done via the switch on an off methods
-
-
-        bool startBus(bool createDevices);
-        void closeBus();
-
-        void updatePDO();
-
-        bool generalSDOWrite(uint16_t slave, uint16_t index, uint8_t subindex, int buflen, void* buf, bool ca);
-
-        bool generalSDORead(uint16_t slave, uint16_t index, uint8_t subindex, int buflen, void* buf, bool ca = false) const;
-
-
-        bool busShouldBeRunning() const;
-
-        std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr> > createDevices();
-
-        bool setPDOMappings();
-
-        void printALStatusError(uint16_t slave);
-
-        void errorHandling();
-
-
-        std::vector<ControlDevicePtr> ctrlDevs;
-        std::vector<SensorDevicePtr> sensDevs;
-
-
-        //members
-        /** the expected working counter that will be calculated at the bus initialisation */
-        int expectedWKC;
-        /** this indicates if the EtherCAT bus is on, will change to FALSE when the modul stopps */
-        std::atomic_bool isSDOWorking;
-        //! Socketfiledescriptor on which the ethercat connection is running
-        int socketFileDescriptor = -1;
-        /** @brief IOmap the IO map where the process data are mapped in */
-        alignas(alignof(std::max_align_t)) std::array<char, IOmapSize> IOmap;
-        /**  @brief switchON_OFF this flag is used to switch the bus off an close it */
-        std::atomic_bool switchON_OFF;
-        /** current Bus group we are working on */
-        int currentGroup;
-        /** indicates if there is an Error */
-        std::atomic_bool error;
-
-        /** List of all Slaves */
-        std::vector<AbstractSlavePtr> slaves;
-        /** flag indicates if the bus is started complete and all slaves are in EtherOP Mode*/
-        std::atomic_bool busInOperationalMode;
-        /** the working counter from the last transmission */
-        int lastWorkCounter;
-        int noErrorIterations = 0;
-        bool firstErrorCheck = true;
-
-        /** List of all functional/internal Units */
-        std::vector<AbstractFunctionalDevicePtr> functionalDevices;
-
-
-        int actualMappedSize;
-
-        std::atomic_bool checkErrorCountersOnWKCError;
-
-        /** Indicates if the bus got broken and is not recoverable, so we don't need to switch it down correct it already went away...*/
-        std::atomic_bool isBusDead;
-
-        std::atomic_bool pdoMapped;
-
-        DeviceContainerPtr deviceContainerPtr;
-        RobotUnit* mainUnitPtr;
-
-        std::string ifname;
-        mutable std::mutex etherCatMutex;
-        bool emergencyStopWasActivated = false;
-        IceUtil::Time busUpdateLastUpdateTime;
-        IceUtil::Time ermergencyStopRecoverStartpoint;
-
-
-    };
-
-
-
-}
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.cpp
deleted file mode 100644
index 9de270766789a036fbae6f5aa2c8ea305f2d7597..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#include "EtherCATDeviceFactory.h"
-#include "AbstractFunctionalDevice.h"
-
-namespace armarx
-{
-
-
-
-
-    EtherCATDeviceFactory::EtherCATDeviceFactory()
-    {
-
-    }
-
-    const std::vector<AbstractSlavePtr>& EtherCATDeviceFactory::getSlaves() const
-    {
-        return slaves;
-    }
-
-    const std::vector<ControlDevicePtr>& EtherCATDeviceFactory::getCtrlDevs() const
-    {
-        return ctrlDevs;
-    }
-
-    const std::vector<SensorDevicePtr>& EtherCATDeviceFactory::getSensDevs() const
-    {
-        return sensDevs;
-    }
-
-    void EtherCATDeviceFactory::addControlDevice(ControlDevicePtr dev)
-    {
-        ctrlDevs.push_back(dev);
-    }
-
-    void EtherCATDeviceFactory::addSensorDevice(SensorDevicePtr dev)
-    {
-        sensDevs.push_back(dev);
-    }
-
-    void EtherCATDeviceFactory::addSlave(const AbstractSlavePtr& slave)
-    {
-        this->slaves.push_back(slave);
-    }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h
deleted file mode 100644
index ee1bfca9a258eaba11052d5d0a997877fb6b8a3c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#pragma once
-#include "ArmarXEtherCATFwd.h"
-#include <ArmarXCore/core/system/AbstractFactoryMethod.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "SlaveIdentifier.h"
-
-
-
-namespace armarx
-{
-    using AbstractFunctionalDevicePtr = std::shared_ptr<class AbstractFunctionalDevice>;
-
-    using DeviceContainerPtr = std::shared_ptr<class DeviceContainer>;
-
-    using ControlDevicePtr = std::shared_ptr<class ControlDevice>;
-    using SensorDevicePtr = std::shared_ptr<class SensorDevice>;
-
-    using EtherCATFactoryArgs = std::tuple<EtherCAT*, uint16_t, DeviceContainerPtr>;
-
-    class EtherCATDeviceFactory :
-        public AbstractFactoryMethod<EtherCATDeviceFactory, EtherCATFactoryArgs, std::shared_ptr<EtherCATDeviceFactory>>
-    {
-    public:
-        EtherCATDeviceFactory();
-        const std::vector<AbstractSlavePtr>& getSlaves() const;
-        const std::vector<ControlDevicePtr>& getCtrlDevs() const;
-        const std::vector<SensorDevicePtr>& getSensDevs() const;
-
-        void addSlave(const AbstractSlavePtr& slave);
-        void addControlDevice(ControlDevicePtr dev);
-        void addSensorDevice(SensorDevicePtr dev);
-    private:
-        std::vector<AbstractSlavePtr> slaves;
-        std::vector<ControlDevicePtr> ctrlDevs;
-        std::vector<SensorDevicePtr> sensDevs;
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
deleted file mode 100644
index a6feb1e98e7eb83f743e0324006ecb878d5dd15b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ /dev/null
@@ -1,821 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#include "EtherCATRTUnit.h"
-#include "DeviceContainer.h"
-#include <chrono>
-#include <thread>
-#include <sstream>
-#include <sched.h>
-#include <syscall.h>
-#include <sys/mman.h>
-#include <sys/stat.h>
-
-#include <boost/algorithm/clamp.hpp>
-
-#include <VirtualRobot/Tools/Gravity.h>
-#include <VirtualRobot/RobotNodeSet.h>
-
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
-
-
-
-#include <RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <sched.h>
-#include <sys/mman.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <syscall.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
-#include <RobotAPI/libraries/core/Pose.h>
-
-
-using namespace armarx;
-
-
-
-#define MAX_SAFE_STACK (8*1024) /* The maximum stack size which is
-                                   guaranteed safe to access without
-                                   faulting */
-
-
-
-#define NSEC_PER_SEC    (1000*1000*1000) /* The number of nsecs per sec. */
-
-
-
-EtherCATRTUnit::EtherCATRTUnit() :
-    rtLoopTime(-1),
-    deviceContainerPtr(nullptr)
-{
-
-}
-
-void EtherCATRTUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
-{
-    if (changedProperties.count("checkErrorCountersOnEtherCATError"))
-    {
-        auto val = getProperty<bool>("checkErrorCountersOnEtherCATError").getValue();
-        ARMARX_INFO << "Changing checkErrorCountersOnEtherCATError to " << val;
-        EtherCAT::getBus().setCheckErrorCountersOnWKCError(val);
-    }
-}
-
-
-void EtherCATRTUnit::onInitRobotUnit()
-{
-    ARMARX_TRACE;
-    rtWarningFactor = getProperty<float>("RTLoopTimingCheckToleranceFactor").getValue();
-    rtLoopTime = getProperty<int>("RTLoopFrequency").getValue();
-
-
-    ARMARX_INFO << "Locking memory";
-
-    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
-    {
-        ARMARX_WARNING << "Could nock lock memory (mlockall failed)";
-        //::exit(-2);
-    }
-
-    ARMARX_DEBUG << "Pre-fault our stack";
-    unsigned char dummy[MAX_SAFE_STACK];
-    memset(dummy, 0, MAX_SAFE_STACK);
-
-    ARMARX_INFO << "EtherCATRTUnit initializing now";
-    publishRobot = rtGetRobot()->clone();
-    ARMARX_CHECK_EXPRESSION(rtGetRobot()->hasRobotNodeSet("RobotJoints"));
-    ARMARX_CHECK_EXPRESSION(rtGetRobot()->hasRobotNodeSet("RobotCol"));
-    rtRobotJointSet = rtGetRobot()->getRobotNodeSet("RobotJoints");
-    rtRobotBodySet = rtGetRobot()->getRobotNodeSet("RobotCol");
-    ARMARX_CHECK_NOT_NULL(rtRobotJointSet);
-    ARMARX_CHECK_NOT_NULL(rtRobotBodySet);
-    for (auto& node : *rtRobotJointSet)
-    {
-        node->setEnforceJointLimits(false);
-    }
-    std::stringstream massesInfo;
-    for (int i = 0; i < (int)rtRobotBodySet->getSize(); ++i)
-    {
-        auto node = rtRobotBodySet->getNode(i);
-        massesInfo << "\t" << node->getName() << ": " << node->getMass() << " kg\n";
-    }
-    ARMARX_DEBUG << "Masses info: " << massesInfo.str();
-
-    //setting the bus
-    EtherCAT& bus = EtherCAT::getBus();
-    bus.setMainUnitPtr(this);
-    bus.setDeviceContainerPtr(deviceContainerPtr);
-    bus.setCheckErrorCountersOnWKCError(getProperty<bool>("checkErrorCountersOnEtherCATError").getValue());
-    checkErrorCountersOnStartupFlag = (getProperty<bool>("checkErrorCountersOnStartup").getValue());
-    checkErrorCountersOnStartupDelayMS = (getProperty<long>("checkErrorCountersOnStartupDelayMS").getValue());
-
-}
-
-void EtherCATRTUnit::onConnectRobotUnit()
-{
-    ARMARX_INFO << "EtherCATRTUnit connects now";
-    dd = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
-
-
-    startRTThread();
-    while (this->getRobotUnitState() < RobotUnitState::Running)
-    {
-        usleep(100000);
-    }
-
-
-
-
-
-
-}
-
-void EtherCATRTUnit::onDisconnectRobotUnit()
-{
-    ARMARX_INFO << "EtherCATRTUnit disconnects now";
-}
-
-void EtherCATRTUnit::onExitRobotUnit()
-{
-    ARMARX_INFO << "EtherCATRTUnit is exiting now ";
-
-    /* close the latency_target_fd if it's open */
-    if (latency_target_fd >= 0)
-    {
-        close(latency_target_fd);
-    }
-    ARMARX_INFO << "EtherCATRTUnit exit complete";
-}
-
-void EtherCATRTUnit::initializeKinematicUnit()
-{
-    using IfaceT = KinematicUnitInterface;
-
-    auto guard = getGuard();
-    throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
-    //check if unit is already added
-    if (getUnit(IfaceT::ice_staticId()))
-    {
-        return;
-    }
-
-    auto unit = createKinematicSubUnit(getIceProperties()->clone(), ControlModes::Position1DoF,
-                                       getProperty<bool>("UseTorqueVelocityModeAsDefault").getValue() ? ControlModes::VelocityTorque : ControlModes::Velocity1DoF);
-    //add
-    if (unit)
-    {
-        addUnit(std::move(unit));
-    }
-}
-
-void EtherCATRTUnit::startRTThread()
-{
-    ARMARX_INFO << "starting control task";
-    //task->start();
-    if (rtTask.joinable())
-    {
-        rtTask.join();
-    }
-
-    rtTask = std::thread
-    {
-        [this] {
-            taskRunning = true;
-            try
-            {
-                this->rtRun();
-            }
-            catch (...)
-            {
-                ARMARX_FATAL << "RT Thread caused an uncaught exception:\n" << GetHandledExceptionString();
-            }
-        }
-    };
-
-}
-
-void EtherCATRTUnit::joinControlThread()
-{
-    ARMARX_INFO << "stopping control task";
-    taskRunning = false;
-    //    EtherCAT& bus = EtherCAT::getBus();
-    //    bus.switchBusOFF();
-    rtTask.join();
-    ARMARX_INFO << "rt task stopped";
-}
-
-void EtherCATRTUnit::publish(StringVariantBaseMap debugObserverMap, StringVariantBaseMap timingMap)
-{
-    RobotUnit::publish(std::move(debugObserverMap), std::move(timingMap));
-
-    //    for (auto& name : getSensorDeviceNames())
-    //    {
-    //        auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorPosition>();
-    //        if (!data)
-    //        {
-    //            continue;
-    //        }
-    //        publishRobot->getRobotNode(name)->setJointValueNoUpdate(data->position);
-    //    }
-    //    publishRobot->applyJointValues();
-    //    for (auto& name : getSensorDeviceNames())
-    //    {
-    //        auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorTorque>();
-    //        if (!data)
-    //        {
-    //            continue;
-    //        }
-    //        auto node = publishRobot->getRobotNode(name);
-    //        ARMARX_CHECK_EXPRESSION(node);
-    //        auto joint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodeRevolute>(node);
-    //        ARMARX_CHECK_EXPRESSION(joint);
-    //        Vector3Ptr pos = new Vector3(joint->getGlobalPosition());
-    //        Vector3Ptr axis = new Vector3(joint->getJointRotationAxis());
-    //        float percent = data->torque / 4.0f;
-    //        if (std::abs(percent) > 0.1)
-    //        {
-    //            getDebugDrawerProxy()->setCircleArrowVisu("TorqueEstimation", name, pos, axis, 100, percent, 5, DrawColor {0, 1, 0, 1});
-    //        }
-    //        else
-    //        {
-    //            getDebugDrawerProxy()->removeCircleVisu("TorqueEstimation", name);
-    //        }
-
-    //    }
-}
-
-armarx::PropertyDefinitionsPtr EtherCATRTUnit::createPropertyDefinitions()
-{
-    return armarx::PropertyDefinitionsPtr(new EtherCATRTUnitPropertyDefinitions(
-            getConfigIdentifier()));
-}
-
-EtherCATRTUnit::~EtherCATRTUnit()
-{
-    ARMARX_INFO << "DTOR of EtherCATRTUnit";
-}
-
-
-void EtherCATRTUnit::rtRun()
-{
-    ARMARX_INFO << "Control task running";
-    EtherCAT& bus = EtherCAT::getBus();
-
-    ARMARX_ON_SCOPE_EXIT
-    {
-        ARMARX_INFO << "Leaving rtRun scope";
-        //switching off the bus and be sure that the bus will receive
-        bus.switchBusOFF();
-        bus.updateBus();
-        if (getProperty<int>("SocketFileDescriptor").isSet() && getProperty<int>("SocketFileDescriptor").getValue() > 0)
-        {
-            ARMARX_INFO << "Closing raw socket with FD " << getProperty<int>("SocketFileDescriptor").getValue();
-            int ret = close(getProperty<int>("SocketFileDescriptor").getValue());
-            if (ret)
-            {
-                ARMARX_INFO << "Failed to close raw socket file handle: " << ret << " errno: " << strerror(errno);
-            }
-        }
-    };
-
-
-    bool busStartSucceeded = false;
-    if (getProperty<bool>("StartEtherCATBus").getValue())
-    {
-        try
-        {
-            ARMARX_DEBUG << "initBus";
-            busStartSucceeded = initBusRTThread();
-        }
-        catch (...)
-        {
-            ARMARX_ERROR << "init ethercat bus failed with exception: " << armarx::GetHandledExceptionString();
-            throw;
-        }
-
-        ARMARX_INFO << "initBus finished with: " << busStartSucceeded;
-    }
-    else
-    {
-        ARMARX_IMPORTANT << "EtherCAT Bus disabled in properties - not starting it";
-    }
-
-    if (deviceContainerPtr->getAllInitializedFunctionalDevices().empty())
-    {
-        ARMARX_WARNING << "No functional devices found - shutting down";
-        return;
-    }
-    ARMARX_DEBUG << "Getting list of uninitialized devices";
-    Ice::StringSeq uninitializedDevices;
-    for (AbstractFunctionalDevicePtr& device : deviceContainerPtr->getAllUninitializedFunctionalDevices())
-    {
-        std::shared_ptr<DeviceBase> deviceBase = std::dynamic_pointer_cast<DeviceBase>(device);
-        if (deviceBase)
-        {
-            uninitializedDevices.push_back(deviceBase->getDeviceName());
-        }
-        else
-        {
-            uninitializedDevices.push_back("Unkown Device");
-        }
-    }
-    if (!uninitializedDevices.empty())
-    {
-        ARMARX_WARNING << "Configured but not found or disabled devices: " << uninitializedDevices;
-    }
-
-
-    bool initializationDone = false;
-    bool initializationFailed = false;
-    ARMARX_DEBUG << "Async init starting now";
-    std::thread unitInitTask = std::thread
-    {
-        [&, this] {
-            try
-            {
-                finishDeviceInitialization();
-                //                rtReadSensorDeviceValues(TimeUtil::GetTime(), IceUtil::Time::microSeconds(1)); // initialize sensor values
-                initializeDefaultUnits();
-                ARMARX_IMPORTANT << "Setting up custom Units";
-                finishUnitInitialization();
-                ARMARX_IMPORTANT << "Finishing setting up custom Units...DONE";
-                ARMARX_INFO << "Sleeping a moment to let everything settle in";
-                usleep(500000);
-                initializationDone = true;
-            }
-            catch (...)
-            {
-                ARMARX_FATAL << "Shutting down - RobotUnit Init Thread caused an uncaught exception:\n" << GetHandledExceptionString();
-                initializationFailed = true;
-
-            }
-        }
-    };
-    unitInitTask.detach();
-    if (initializationFailed)
-    {
-        return;
-    }
-    if (busStartSucceeded)
-    {
-        elevateThreadPriority(RT_THREAD_PRIORITY);
-        //        set_latency_target();
-
-        //setting the timestamps for the pdo update, at the moment we only have on
-        currentPDOUpdateTimestamp = TimeUtil::GetTime();
-        CycleUtil cycleStats(IceUtil::Time::microSeconds(rtLoopTime));
-        cycleStats.setBusyWaitShare(0.1);
-        while (!initializationDone)
-        {
-            //            auto realDeltaT = currentPDOUpdateTimestamp - lastPDOUpdateTimestamp;
-            //            auto cappedDeltaT = IceUtil::Time::microSeconds(boost::algorithm::clamp(realDeltaT.toMicroSeconds(), 1, 2000)); // TODO: Make property
-            bus.updateBus(false);
-            //            rtReadSensorDeviceValues(currentPDOUpdateTimestamp, cappedDeltaT);
-            //            lastPDOUpdateTimestamp = currentPDOUpdateTimestamp;
-            //            currentPDOUpdateTimestamp = TimeUtil::GetTime();
-            cycleStats.waitForCycleDuration();
-            ARMARX_INFO << deactivateSpam(1) << "Waiting for unit initialization!";
-        }
-        ARMARX_IMPORTANT << "RobotUnit is now ready";
-    }
-    else
-    {
-
-        if (!busStartSucceeded && getProperty<bool>("StartEtherCATBus").getValue())
-        {
-            ARMARX_WARNING << "Bus was not started!";
-        }
-    }
-
-    ARMARX_DEBUG << "Setting up gravity calculation";
-    // init data structs for gravity calculation
-    for (size_t i = 0; i < rtRobotJointSet->getSize(); i++)
-    {
-        auto selectedJoint = getSensorDevice(rtRobotJointSet->getNode(i)->getName());
-        if (selectedJoint)
-        {
-            auto sensorValue = const_cast<SensorValue1DoFGravityTorque*>(selectedJoint->getSensorValue()->asA<SensorValue1DoFGravityTorque>());
-            ARMARX_DEBUG << "will calculate gravity for robot node " << rtRobotJointSet->getNode(i)->getName();
-            nodeJointDataVec.push_back(std::make_pair(rtRobotJointSet->getNode(i), sensorValue));
-        }
-        else
-        {
-            ARMARX_INFO << "Joint " << rtRobotJointSet->getNode(i)->getName() << " not found";
-            nodeJointDataVec.push_back(std::make_pair(rtRobotJointSet->getNode(i), nullptr));
-        }
-    }
-
-    //    unitInitTask.join();
-    controlLoopRTThread();
-    //    //switching off the bus and be sure that the bus will receive
-    //    bus.switchBusOFF();
-    //    bus.updateBus();
-
-    while (getObjectScheduler() && !getObjectScheduler()->isTerminationRequested())
-    {
-        ARMARX_INFO << deactivateSpam() << "Waiting for shutdown";
-        usleep(10000);
-    }
-}
-
-void EtherCATRTUnit::icePropertiesInitialized()
-{
-    RobotUnit::icePropertiesInitialized();
-
-}
-
-MultiNodeRapidXMLReader EtherCATRTUnit::ReadHardwareConfigFile(std::string busConfigFilePath, std::string rootNodeName)
-{
-    if (!ArmarXDataPath::getAbsolutePath(busConfigFilePath, busConfigFilePath))
-    {
-        throw LocalException("could not find BusConfigFilePath: ") << busConfigFilePath;
-    }
-    ARMARX_INFO_S << "read the hw config from " << busConfigFilePath;
-
-    //reading the config for the Bus and create all the robot objects in the robot container
-    auto busConfigFilePathDir = std::filesystem::path(busConfigFilePath).parent_path();
-    auto rapidXmlReaderPtr = RapidXmlReader::FromFile(busConfigFilePath);
-    auto rootNode = rapidXmlReaderPtr->getRoot(rootNodeName.c_str());
-    MultiNodeRapidXMLReader multiNode({rootNode});
-    for (RapidXmlReaderNode& includeNode  : rootNode.nodes("include"))
-    {
-        auto relPath = includeNode.attribute_value("file");
-        std::filesystem::path filepath = (busConfigFilePathDir / relPath);
-        if (!std::filesystem::exists(filepath))
-        {
-            std::string absPath;
-            if (!ArmarXDataPath::getAbsolutePath(relPath, absPath))
-            {
-                throw LocalException("Could not find config file at path ") << relPath;
-            }
-        }
-        multiNode.addNode(RapidXmlReader::FromFile(filepath.string())->getRoot(rootNodeName.c_str()));
-    }
-    return multiNode;
-}
-
-bool EtherCATRTUnit::initBusRTThread()
-{
-    // init EtherCAT
-    EtherCAT& bus = EtherCAT::getBus();
-    bus.setSocketFileDescriptor(getProperty<int>("SocketFileDescriptor").getValue());
-    bus.setIfName(getProperty<std::string>("EthercatInterfaceName"));
-    if (!bus.switchBusON())
-    {
-        ARMARX_INFO << "Starting bus failed!! - closing\n";
-        return false;
-    }
-    for (auto& ctrl : bus.getCtrlDevs())
-    {
-        addControlDevice(ctrl);
-    }
-    for (auto& sens : bus.getSensDevs())
-    {
-        addSensorDevice(sens);
-    }
-    ARMARX_INFO << "EtherCAT bus is started";
-    return true;
-}
-
-void EtherCATRTUnit::controlLoopRTThread()
-{
-    EtherCAT& bus = EtherCAT::getBus();
-    try
-    {
-        finishControlThreadInitialization();
-
-        int pid = syscall(SYS_gettid);
-        ARMARX_IMPORTANT << "pinning thread " << pid << " to cpu #0";
-        cpu_set_t mask;
-        CPU_ZERO(&mask);
-        CPU_SET(0, &mask);
-        int retval = sched_setaffinity(pid, sizeof(mask), &mask);
-        //        int retval = system(("taskset -pc 0 " + to_string(pid)).c_str());
-        if (retval != 0)
-        {
-            ARMARX_ERROR << "Failed to pin thread " << pid << " to cpu #0";
-        }
-        cpu_set_t mask2;
-        CPU_ZERO(&mask2);
-        CPU_SET(0, &mask2);
-        sched_getaffinity(pid, sizeof(mask2), &mask2);
-        ARMARX_INFO << "Thread Pinning after matches mask: " << CPU_EQUAL(&mask, &mask2);
-
-        //bus.setControlLoopRunning(true);
-        //        rtLoopStartTime = TimeUtil::GetTime();
-        //to not get any strange start values
-        currentPDOUpdateTimestamp = armarx::rtNow();
-        CycleUtil cycleKeeper(IceUtil::Time::microSeconds(rtLoopTime));
-        cycleKeeper.setBusyWaitShare(1.0f);
-        ARMARX_CHECK_EXPRESSION(rtGetRobot());
-        ARMARX_CHECK_EXPRESSION(rtRobotJointSet);
-        ARMARX_CHECK_EXPRESSION(rtRobotBodySet);
-        ARMARX_CHECK_EXPRESSION(rtRobotJointSet->getSize() > 0);
-        VirtualRobot::Gravity gravity(rtGetRobot(), rtRobotJointSet, rtRobotBodySet);
-
-        //#if 0
-        std::vector<float> gravityValues(rtRobotJointSet->getSize());
-        ARMARX_CHECK_EQUAL(rtRobotJointSet->getSize(), nodeJointDataVec.size());
-        //#endif
-
-        ARMARX_INFO << "RT control loop started";
-        EmergencyStopState lastSoftwareEmergencyStopState = rtGetEmergencyStopState();
-
-        //        rtLoopStartTime = TimeUtil::GetTime();
-        auto lastMonoticTimestamp = armarx::rtNow();
-        auto currentMonotonicTimestamp = lastMonoticTimestamp;
-        currentPDOUpdateTimestamp = armarx::rtNow();
-
-        IceUtil::Time startTimestamp = armarx::rtNow();
-
-        for (; taskRunning; ++ _iterationCount)
-        {
-            const IceUtil::Time loopStartTime = armarx::rtNow();
-            rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopStart();
-
-            if (checkErrorCountersOnStartupFlag && (loopStartTime - startTimestamp).toMilliSeconds() > checkErrorCountersOnStartupDelayMS)
-            {
-                checkErrorCountersOnStartupFlag = false;
-                bus.readErrorCounters();
-            }
-            if (checkErrorCountersTriggerFlag)
-            {
-                checkErrorCountersTriggerFlag = false;
-                bus.readErrorCounters();
-            }
-
-            auto realDeltaT = currentMonotonicTimestamp - lastMonoticTimestamp;
-            auto cappedDeltaT = IceUtil::Time::microSeconds(boost::algorithm::clamp<long>(realDeltaT.toMicroSeconds(), 1, 2000)); // TODO: Make property
-            //            TIMING_START(load);
-            //            if (controllerLoaded)
-            if (rtIsCalibrating())
-            {
-                rtCalibrateActivateControlers();
-                rtSwitchControllerSetup(SwitchControllerMode::RTThread);
-                rtResetAllTargets();
-                _calibrationStatus = rtCalibrate();
-                rtHandleInvalidTargets();
-                rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
-            }
-            else
-            {
-                RT_TIMING_START(RunControllers);
-                RT_TIMING_START(SwitchControllers);
-                rtSwitchControllerSetup();
-                RT_TIMING_CEND(SwitchControllers, 0.3 * rtWarningFactor);
-                RT_TIMING_START(ResettingTargets);
-                rtResetAllTargets();
-                RT_TIMING_CEND(ResettingTargets, 0.3 * rtWarningFactor);
-                RT_TIMING_START(RunNJointControllers);
-                rtRunNJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
-                RT_TIMING_CEND(RunNJointControllers, 0.3 * rtWarningFactor);
-                RT_TIMING_START(CheckInvalidTargets);
-                rtHandleInvalidTargets();
-                RT_TIMING_CEND(CheckInvalidTargets, 0.3 * rtWarningFactor);
-
-                RT_TIMING_START(RunJointControllers);
-                rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
-                RT_TIMING_CEND(RunJointControllers, 0.3 * rtWarningFactor);
-                RT_TIMING_CEND(RunControllers, 0.3 * rtWarningFactor);
-            }
-
-            //bus update
-            rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveStart();
-            RT_TIMING_START(updateBus);
-            currentPDOUpdateTimestamp = TimeUtil::GetTime();
-            if (bus.isBusInOperationalMode())
-            {
-                // error correction
-                auto currentSoftwareEmergencyStopState = rtGetEmergencyStopState();
-                if (lastSoftwareEmergencyStopState == EmergencyStopState::eEmergencyStopActive && currentSoftwareEmergencyStopState == EmergencyStopState::eEmergencyStopInactive)
-                {
-                    //                    bus.rebootBus();
-                }
-                lastSoftwareEmergencyStopState = currentSoftwareEmergencyStopState;
-
-                bus.updateBus();
-                if (bus.isEmergencyStopActive())
-                {
-                    rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
-                }
-            }
-            RT_TIMING_CEND(updateBus, 0.7 * rtWarningFactor);
-
-            rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveEnd();
-
-            RT_TIMING_START(ReadSensorValues);
-            rtReadSensorDeviceValues(currentPDOUpdateTimestamp, cappedDeltaT);
-            RT_TIMING_CEND(ReadSensorValues, 0.7 * rtWarningFactor);
-            lastMonoticTimestamp = currentMonotonicTimestamp;
-            currentMonotonicTimestamp = armarx::rtNow();
-
-
-            RT_TIMING_START(Publish);
-            rtUpdateSensorAndControlBuffer(currentPDOUpdateTimestamp, cappedDeltaT);
-            RT_TIMING_CEND(Publish, 0.15 * rtWarningFactor);
-
-            RT_TIMING_START(ComputeGravityTorques);
-            gravity.computeGravityTorque(gravityValues);
-            size_t i = 0;
-            for (auto& node : nodeJointDataVec)
-            {
-                auto torque = gravityValues.at(i);
-                if (node.second)
-                {
-                    node.second->gravityTorque = -torque;
-                }
-
-                i++;
-            }
-            RT_TIMING_CEND(ComputeGravityTorques, 0.2 * rtWarningFactor);
-
-            //            copyEtherCATBufferOut();
-
-            rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopPreSleep();
-
-            const auto loopPreSleepTime = armarx::rtNow();
-            RT_TIMING_START(RTLoopWaiting);
-            cycleKeeper.waitForCycleDuration();
-            RT_TIMING_CEND(RTLoopWaiting, rtLoopTime * 1.3 * rtWarningFactor);
-            const auto loopPostSleepTime = armarx::rtNow();
-
-            const auto loopDuration = armarx::rtNow() - loopStartTime;
-            if (loopDuration.toMicroSeconds() > (rtLoopTime + 500) * rtWarningFactor)
-            {
-                const SensorValueRTThreadTimings* sval = rtGetRTThreadTimingsSensorDevice().getSensorValue()->asA<SensorValueRTThreadTimings>();
-                ARMARX_CHECK_NOT_NULL(sval);
-                ARMARX_WARNING << "RT loop is under 1kHz control frequency:\n"
-                               << "-- cycle time PDO timestamp " << realDeltaT.toMicroSeconds() << " µs\n"
-                               << "-- cycle time loop          " << loopDuration.toMicroSeconds() << " µs\n"
-                               << "-- sleep                    " << (loopPostSleepTime - loopPreSleepTime).toMicroSecondsDouble() << " µs\n"
-
-                               << "-- thread timing sensor value: \n"
-
-                               << "---- rtSwitchControllerSetup        Duration " << sval->rtSwitchControllerSetupDuration.toMicroSecondsDouble() << " µs\t"
-                               <<                               " RoundTripTime " << sval->rtSwitchControllerSetupRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
-                               << "---- rtRunNJointControllers         Duration " << sval->rtRunNJointControllersDuration.toMicroSecondsDouble() << " µs\t"
-                               <<                               " RoundTripTime " << sval->rtRunNJointControllersRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
-                               << "---- rtHandleInvalidTargets         Duration " << sval->rtHandleInvalidTargetsDuration.toMicroSecondsDouble() << " µs\t"
-                               <<                               " RoundTripTime " << sval->rtHandleInvalidTargetsRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
-                               << "---- rtRunJointControllers          Duration " << sval->rtRunJointControllersDuration.toMicroSecondsDouble() << " µs\t"
-                               <<                               " RoundTripTime " << sval->rtRunJointControllersRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
-                               << "---- rtBusSendReceive               Duration " << sval->rtBusSendReceiveDuration.toMicroSecondsDouble() << " µs\t"
-                               <<                               " RoundTripTime " << sval->rtBusSendReceiveRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
-                               << "---- rtReadSensorDeviceValues       Duration " << sval->rtReadSensorDeviceValuesDuration.toMicroSecondsDouble() << " µs\t"
-                               <<                               " RoundTripTime " << sval->rtReadSensorDeviceValuesRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
-                               << "---- rtUpdateSensorAndControlBuffer Duration " << sval->rtUpdateSensorAndControlBufferDuration.toMicroSecondsDouble() << " µs\t"
-                               <<                               " RoundTripTime " << sval->rtUpdateSensorAndControlBufferRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
-                               << "---- rtResetAllTargets              Duration " << sval->rtResetAllTargetsDuration.toMicroSecondsDouble() << " µs\t"
-                               <<                               " RoundTripTime " << sval->rtResetAllTargetsRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
-                               << "---- rtLoop                         Duration " << sval->rtLoopDuration.toMicroSecondsDouble() << " µs\t"
-                               <<                               " RoundTripTime " << sval->rtLoopRoundTripTime.toMicroSecondsDouble() << " µs\n";
-            }
-        }
-        ARMARX_IMPORTANT << "RT loop stopped";
-        ARMARX_INFO << "Execution stats: Average: " << cycleKeeper.getAverageDuration().toMilliSecondsDouble()
-                    << " max: " << cycleKeeper.getMaximumDuration().toMilliSecondsDouble()
-                    << " min: " << cycleKeeper.getMinimumDuration().toMilliSecondsDouble();
-        //switching off the bus and be sure that the bus will receive
-
-    }
-    catch (UserException& e)
-    {
-        ARMARX_FATAL << "exception in control thread!"
-                     << "\nwhat:\n" << e.what()
-                     << "\nreason:\n" << e.reason
-                     << "\n\tname: " << e.ice_id()
-                     << "\n\tfile: " << e.ice_file()
-                     << "\n\tline: " << e.ice_line()
-                     << "\n\tstack: " << e.ice_stackTrace()
-                     << std::flush;
-        //TODO emergency stop
-    }
-    catch (Ice::Exception& e)
-    {
-        ARMARX_FATAL << "exception in control thread!\nwhat:\n"
-                     << e.what()
-                     << "\n\tname: " << e.ice_id()
-                     << "\n\tfile: " << e.ice_file()
-                     << "\n\tline: " << e.ice_line()
-                     << "\n\tstack: " << e.ice_stackTrace()
-                     << std::flush;
-        //TODO emergency stop
-    }
-    catch (std::exception& e)
-    {
-        ARMARX_FATAL << "exception in control thread!\nwhat:\n" << e.what() << std::flush;
-        //TODO emergency stop
-    }
-    catch (...)
-    {
-        ARMARX_FATAL << "exception in control thread!" << std::flush;
-        //TODO emergency stop
-    }
-    ARMARX_INFO << "Leaving control loop function";
-}
-
-DeviceContainerPtr EtherCATRTUnit::getDeviceContainerPtr() const
-{
-    return deviceContainerPtr;
-}
-
-void EtherCATRTUnit::setDeviceContainerPtr(const DeviceContainerPtr& value)
-{
-    deviceContainerPtr = value;
-}
-
-
-
-void EtherCATRTUnit::elevateThreadPriority(int priority)
-{
-    int pid = syscall(SYS_gettid);
-    ARMARX_INFO << "Priority before: " << sched_getscheduler(pid);
-    struct sched_param param;
-    param.sched_priority = priority;
-    if (sched_setscheduler(LogSender::getThreadId(), SCHED_FIFO | SCHED_RESET_ON_FORK, &param) == -1)
-    {
-        ARMARX_WARNING << ("sched_setscheduler failed");
-        //::exit(-1);
-    }
-    ARMARX_INFO << "Priority: " << sched_getscheduler(pid);
-
-}
-
-/* Latency trick
- * if the file /dev/cpu_dma_latency exists,
- * open it and write a zero into it. This will tell
- * the power management system not to transition to
- * a high cstate (in fact, the system acts like idle=poll)
- * When the fd to /dev/cpu_dma_latency is closed, the behavior
- * goes back to the system default.
- *
- * Taken from rt-tests.
- */
-void EtherCATRTUnit::set_latency_target(int32_t latency_target_value)
-{
-
-    struct stat s;
-    int err;
-    errno = 0;
-    err = stat("/dev/cpu_dma_latency", &s);
-    if (err == -1)
-    {
-        ARMARX_WARNING << "WARN: stat /dev/cpu_dma_latency failed";
-        return;
-    }
-    errno = 0;
-    latency_target_fd = open("/dev/cpu_dma_latency", O_RDWR);
-    if (latency_target_fd == -1)
-    {
-        ARMARX_WARNING << "WARN: open /dev/cpu_dma_latency failed: " << strerror(errno);
-        return;
-    }
-    errno = 0;
-    err = write(latency_target_fd, &latency_target_value, 4);
-    if (err < 1)
-    {
-        ARMARX_WARNING << "# error setting cpu_dma_latency to latency_target_value!";
-        close(latency_target_fd);
-        return;
-    }
-    ARMARX_INFO <<  "# /dev/cpu_dma_latency set to " << latency_target_value << " µs\n";
-}
-
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
deleted file mode 100644
index dcda1ceb3cb6907ef3facd1ef7ce5301b6c502bf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
+++ /dev/null
@@ -1,223 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-
-#include <array>
-#include <thread>
-
-//EVAL some stuff for logging
-#include <sstream>
-#include <fstream>
-
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/util/TripleBuffer.h>
-#include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-
-#include <RobotAPI/components/units/SensorActorUnit.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include "ArmarXEtherCATFwd.h"
-
-#define RT_THREAD_PRIORITY (49) /* we use 49 as the PRREMPT_RT use 50
-                            as the priority of kernel tasklets
-                            and interrupt handler by default */
-
-
-namespace armarx
-{
-
-    /**
-     * @class EtherCATRTUnitPropertyDefinitions
-     * @brief
-     */
-    class EtherCATRTUnitPropertyDefinitions:
-        public armarx::RobotUnitPropertyDefinitions
-    {
-    public:
-        EtherCATRTUnitPropertyDefinitions(std::string prefix):
-            armarx::RobotUnitPropertyDefinitions(prefix)
-        {
-            //defineRequiredProperty<std::string>("PropertyName", "Description");
-            //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
-            defineRequiredProperty<std::string>("BusConfigFilePath", "Location of the BusConfigFile");
-            defineOptionalProperty<int>("SocketFileDescriptor", 777, "Socketfiledescriptor on which the ethercat connection is running");
-            defineOptionalProperty<std::string>("EthercatInterfaceName", "", "Name of the ethercat socket. If set to non-empty string, this will be used over SocketFileDescriptor");
-            defineOptionalProperty<bool>("StartEtherCATBus", true, "Whether or not to start the EtherCAT Bus. Useful if only the Dynamixels should be used.");
-
-            defineOptionalProperty<bool>("UseTorqueVelocityModeAsDefault", false, "If true, the KinematicUnit will use TorqueVelocity mode for velocity mode");
-            defineOptionalProperty<int>("RTLoopFrequency", 1000, "Desired frequency of real-time control loop");
-            defineOptionalProperty<float>("RTLoopTimingCheckToleranceFactor", 1.0f, "Factor by which the timing checks are multiplied. Higher value -> less warning outputs");
-            defineOptionalProperty<bool>("checkErrorCountersOnEtherCATError", false, "If true, the EtherCAT bus will be checked for receive errors on bus error. This is slow; it should not be active in normal usage.", PropertyDefinitionBase::eModifiable);
-            defineOptionalProperty<bool>("checkErrorCountersOnStartup", false, "If true all error counter registers are read on bus startup.");
-            defineOptionalProperty<long>("checkErrorCountersOnStartupDelayMS", 1000, "Delay for the initial error counter check in ms.");
-
-
-            defineOptionalProperty<bool>("VisualizeTorques", false, "If true, EtherCATRTUnit::publish will draw joint torques on the debug drawer");
-        }
-    };
-
-    /**
-     * @defgroup Component-EtherCATRTUnit EtherCATRTUnit
-     * @ingroup RobotAPI-Components
-     * A description of the component EtherCATRTUnit.
-     *
-     * @class EtherCATRTUnit
-     * @ingroup Component-EtherCATRTUnit
-     * @brief Brief description of class EtherCATRTUnit.
-     *
-     * Detailed description of class EtherCATRTUnit.
-     */
-    class EtherCATRTUnit :
-        virtual public Logging,
-        virtual public RobotUnit
-    {
-    public:
-        EtherCATRTUnit();
-        ~EtherCATRTUnit() override;
-
-        /**
-         * @see armarx::ManagedIceObject::getDefaultName()
-         */
-        std::string getDefaultName() const override
-        {
-            return "EtherCATRTUnit";
-        }
-
-
-        void elevateThreadPriority(int priority);
-        DeviceContainerPtr getDeviceContainerPtr() const;
-
-    protected:
-        void setDeviceContainerPtr(const DeviceContainerPtr& value);
-
-        void onInitRobotUnit() override;
-        void onConnectRobotUnit() override;
-        void onDisconnectRobotUnit() override;
-        void onExitRobotUnit() override;
-
-        void initializeKinematicUnit() override;
-
-        void joinControlThread() override;
-
-        void publish(armarx::StringVariantBaseMap debugObserverMap = {}, armarx::StringVariantBaseMap timingMap = {}) override;
-
-        /**
-         * @see PropertyUser::createPropertyDefinitions()
-         */
-        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-        void icePropertiesInitialized() override;
-
-        void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
-
-    protected:
-        static MultiNodeRapidXMLReader ReadHardwareConfigFile(std::string hardwareConfigFilePath, std::string rootNodeName);
-
-        //all the stuff to run the rt Thread
-        void startRTThread();
-
-        //        void stopRTThread();
-
-        /** the run method of the rt thread */
-        virtual void rtRun();
-
-        bool initBusRTThread();
-
-        void controlLoopRTThread();
-
-        enum class CalibrationStatus
-        {
-            Calibrating, Done
-        };
-        /**
-         * @brief Allows to switch controllers while calibrating
-         *
-         * use
-         * rtSetJointController(JointController* c, std::size_t index)
-         * to switch controllers
-         */
-        virtual void rtCalibrateActivateControlers()
-        {
-        }
-        /**
-         * @brief Hook to add calibration code
-         *
-         * This function is called in the rt loop! So you should not take too long!
-         *
-         * read sensors and write targets
-         * while calibrating return CalibrationStatus::Calibrating
-         * if done return CalibrationStatus::Done
-         *
-         * @return Whether done or still calibrating
-         */
-        virtual CalibrationStatus rtCalibrate()
-        {
-            return CalibrationStatus::Done;
-        }
-        bool rtIsCalibrating() const
-        {
-            return _calibrationStatus == CalibrationStatus::Calibrating;
-        }
-        std::uintmax_t getIterationCount()
-        {
-            return _iterationCount;
-        }
-    private:
-        CalibrationStatus _calibrationStatus = CalibrationStatus::Calibrating;
-        std::atomic_uintmax_t _iterationCount = 0;
-    protected:
-
-        void computeInertiaTorque();
-        DebugDrawerInterfacePrx dd;
-
-        std::thread rtTask;
-        std::atomic_bool taskRunning  {false};
-        std::atomic_int rtLoopTime{1000};
-        float rtWarningFactor{1};
-
-
-        //timestamps for the pdo updates
-        IceUtil::Time currentPDOUpdateTimestamp;
-
-        VirtualRobot::RobotPtr publishRobot;
-        DeviceContainerPtr deviceContainerPtr;
-
-        VirtualRobot::RobotNodeSetPtr rtRobotJointSet, rtRobotBodySet;
-        std::vector<std::pair<VirtualRobot::RobotNodePtr, SensorValue1DoFGravityTorque*>> nodeJointDataVec;
-        int latency_target_fd = -1;
-
-        void set_latency_target(int32_t latency_target_value = 0);
-
-        IceUtil::Time getControlThreadTargetPeriod() const override
-        {
-            return IceUtil::Time::microSeconds(rtLoopTime);
-        }
-
-        std::atomic_bool checkErrorCountersTriggerFlag = false;
-        std::atomic_bool checkErrorCountersOnStartupFlag = false;
-        std::atomic_long checkErrorCountersOnStartupDelayMS = 0;
-    };
-}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp
deleted file mode 100644
index 8dca1b8e84979538be9fead1a60af32ee7bb0a22..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "SlaveIdentifier.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
-
-using namespace armarx;
-
-SlaveIdentifier::SlaveIdentifier(const RapidXmlReaderNode& node)
-{
-    ARMARX_TRACE;
-    ARMARX_DEBUG << "node valid " << node.is_valid()
-                 << "\npaths: " << node.getChildPaths();
-    ARMARX_CHECK_EXPRESSION(
-        (
-            node.has_node("VendorID") &&
-            node.has_node("ProductID") &&
-            node.has_node("Serial")
-        ) ||
-        node.has_node("Identifier")
-    );
-    if (node.has_node("Identifier"))
-    {
-        const auto inode = node.first_node("Identifier");
-        ARMARX_CHECK_EXPRESSION(
-            inode.has_node("VendorID") &&
-            inode.has_node("ProductID") &&
-            inode.has_node("Serial")
-        ) << "paths: " << inode.getChildPaths();
-        VendorID  = inode.first_node("VendorID").value_as_uint32();
-        ProductID = inode.first_node("ProductID").value_as_uint32();
-        Serial    = inode.first_node("Serial").value_as_uint32();
-    }
-    else
-    {
-        VendorID  = node.first_node("VendorID").value_as_uint32();
-        ProductID = node.first_node("ProductID").value_as_uint32();
-        Serial    = node.first_node("Serial").value_as_uint32();
-    }
-}
-
-SlaveIdentifier::SlaveIdentifier(uint32_t VendorID, uint32_t ProductID, uint32_t Serial, const std::string& humanName)
-    : VendorID(VendorID), ProductID(ProductID), Serial(Serial), humanName(humanName)
-{
-
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h b/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h
deleted file mode 100644
index 50f1d5401d436c08dbb38896a15e858728c48a82..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#pragma once
-
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-
-namespace armarx
-{
-    class SlaveIdentifier;
-    using SlaveIdentifierPtr = std::shared_ptr<SlaveIdentifier>;
-
-    class SlaveIdentifier
-    {
-    public:
-        SlaveIdentifier(const RapidXmlReaderNode& node);
-        SlaveIdentifier(uint32_t VendorID, uint32_t ProductID, uint32_t Serial, const std::string& humanName);
-
-        uint32_t VendorID;
-        uint32_t ProductID;
-        uint32_t Serial;
-        std::string humanName;
-
-        friend std::ostream& operator<<(std::ostream& stream, const SlaveIdentifier& rhs)
-        {
-            stream << "Name: " << rhs.humanName << " Product ID: " << rhs.ProductID << " Serial: " << rhs.Serial << " VendorID: " << rhs.VendorID;
-            return stream;
-        }
-    };
-
-    class DXLIdentifier
-    {
-    public:
-        DXLIdentifier(const RapidXmlReaderNode& node)
-        {
-            dxlID = node.first_node("DynamixelID").value_as_uint32();
-        }
-
-        DXLIdentifier(uint8_t dxl_id, const std::string& humanName) :
-            dxlID(dxl_id),
-            humanName(humanName)
-        {
-
-        }
-
-        uint8_t dxlID;
-        std::string humanName;
-
-
-        friend std::ostream& operator<<(std::ostream& stream, const DXLIdentifier& rhs)
-        {
-            stream << "Name: " << rhs.humanName << " dxlID: " << (int)rhs.dxlID;
-            return stream;
-        }
-    };
-}
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h b/source/RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h
deleted file mode 100644
index 72b7738fa5a0890712b8dbcdfa70f11179718ed4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <memory>
-
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <ArmarXCore/core/system/AbstractFactoryMethod.h>
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/Robot.h>
-
-
-namespace armarx
-{
-    using AbstractFunctionalDevicePtr = std::shared_ptr<class AbstractFunctionalDevice>;
-
-    using VirtualDeviceFactoryArgs = std::tuple<RapidXmlReaderNode,  armarx::DefaultRapidXmlReaderNode, VirtualRobot::RobotPtr>;
-
-    class VirtualDeviceFactory :
-        public AbstractFactoryMethod<VirtualDeviceFactory, VirtualDeviceFactoryArgs, AbstractFunctionalDevicePtr>
-    {
-    public:
-        template <typename ObjectType>
-        static VirtualDeviceFactory::SharedPointerType createInstance(VirtualDeviceFactoryArgs args)
-        {
-            return VirtualDeviceFactory::SharedPointerType(std::make_shared<ObjectType>(std::get<0>(args), std::get<1>(args), std::get<2>(args)));
-        }
-    };
-
-
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/test/ArmarXEtherCATTest.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/test/ArmarXEtherCATTest.cpp
deleted file mode 100644
index eaba09e62c723e33183dc0ae53119d7e29fa855b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/test/ArmarXEtherCATTest.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ReactiveGrasping::ArmarXObjects::ArmarXEtherCAT
- * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date       2016
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#define BOOST_TEST_MODULE ReactiveGrasping::ArmarXLibraries::ArmarXEtherCAT
-
-#define ARMARX_BOOST_TEST
-
-#include <ReactiveGrasping/Test.h>
-#include "../ArmarXEtherCAT.h"
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
-    BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/test/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXEtherCAT/test/CMakeLists.txt
deleted file mode 100644
index fe94a05051e7784f5320c3fb3f183bf1f4238e92..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-#SET(LIBS ${LIBS} ArmarXCore ArmarXEtherCAT)
- 
-#armarx_add_test(ArmarXEtherCATTest ArmarXEtherCATTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index cd4ba3e2ee63cfd902e4f3324aba47ebcd6d9e71..4eccd5e5c5b8753b926be68500cdafaff4497a70 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,14 +1,10 @@
-add_subdirectory(ArmarXEtherCAT)
+
 add_subdirectory(ArmarXObjects)
 add_subdirectory(PriorKnowledge)
-add_subdirectory(ControllerUIUtility)
 add_subdirectory(core)
 add_subdirectory(DebugDrawerConfigWidget)
 # disabled until Mathlib include is fixed...
-add_subdirectory(DMPController)
-add_subdirectory(DSControllers)
 add_subdirectory(RobotAPIComponentPlugins)
-add_subdirectory(RobotAPINJointControllers)
 add_subdirectory(RobotAPIVariantWidget)
 add_subdirectory(RobotStatechartHelpers)
 add_subdirectory(SimpleJsonLogger)
@@ -30,7 +26,5 @@ add_subdirectory(armem_motions)
 add_subdirectory(armem_mps)
 add_subdirectory(aron)
 
-add_subdirectory(NJointControllerGuiPluginUtility)
-add_subdirectory(RobotAPINJointControllerWidgets)
 add_subdirectory(RobotUnitDataStreamingReceiver)
 add_subdirectory(GraspingUtility)
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CMakeLists.txt b/source/RobotAPI/libraries/ControllerUIUtility/CMakeLists.txt
deleted file mode 100644
index 9f66351571637b063f8eab2328b58634f76965b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/CMakeLists.txt
+++ /dev/null
@@ -1,20 +0,0 @@
-set(LIB_NAME       RobotAPIControllerUIUtility)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-set(LIBS ArmarXCore RobotAPIInterfaces RemoteGui)
-
-set(LIB_FILES
-    CartesianWaypointControllerConfig/RemoteGui.h
-    NJointCartesianWaypointControllerConfig/RemoteGui.h
-)
-set(LIB_HEADERS
-    CartesianWaypointControllerConfig/RemoteGui.cpp
-    NJointCartesianWaypointControllerConfig/RemoteGui.cpp
-)
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
deleted file mode 100644
index a568893eb8448825f471ceb44f31944e3bdab56b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
+++ /dev/null
@@ -1,179 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2019
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "RemoteGui.h"
-
-namespace armarx::RemoteGui
-{
-    detail::GroupBoxBuilder makeConfigGui(
-        const std::string& name,
-        const CartesianWaypointControllerConfig& val)
-    {
-        return RemoteGui::makeGroupBox(name)
-               .addChild(
-                   RemoteGui::makeSimpleGridLayout().cols(10)
-
-                   .addChild(RemoteGui::makeTextLabel("Max accelerations:"))
-                   .addChild(RemoteGui::makeTextLabel("Pos:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_maxAcc_Pos")
-                       .min(0)
-                       .max(10000)
-                       .value(val.maxPositionAcceleration)
-                       .decimals(3)
-                   )
-                   .addChild(RemoteGui::makeTextLabel("Ori:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_maxAcc_Ori")
-                       .min(0)
-                       .max(10)
-                       .value(val.maxOrientationAcceleration)
-                       .decimals(3)
-                   )
-                   .addChild(RemoteGui::makeTextLabel("Nullspace:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_maxAcc_Null")
-                       .min(0)
-                       .max(10)
-                       .value(val.maxNullspaceAcceleration)
-                       .decimals(3)
-                   )
-                   .addChild(new RemoteGui::Widget())
-                   .addChild(new RemoteGui::Widget())
-                   .addChild(new RemoteGui::HSpacer)
-
-                   .addChild(RemoteGui::makeTextLabel("JointLimitAvoidance:"))
-                   .addChild(RemoteGui::makeTextLabel("KP:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_KP")
-                       .min(0)
-                       .max(10)
-                       .value(val.kpJointLimitAvoidance)
-                       .decimals(3)
-                   )
-                   .addChild(RemoteGui::makeTextLabel("Scale:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_Scale")
-                       .min(0)
-                       .max(10)
-                       .value(val.jointLimitAvoidanceScale)
-                       .decimals(3)
-                   )
-                   .addChild(new RemoteGui::Widget())
-                   .addChild(new RemoteGui::Widget())
-                   .addChild(new RemoteGui::Widget())
-                   .addChild(new RemoteGui::Widget())
-                   .addChild(new RemoteGui::HSpacer)
-
-                   .addChild(RemoteGui::makeTextLabel("Position:"))
-                   .addChild(RemoteGui::makeTextLabel("Near:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Near")
-                       .min(0)
-                       .max(1000)
-                       .value(val.thresholdPositionNear)
-                       .decimals(3)
-                   )
-                   .addChild(RemoteGui::makeTextLabel("Reached:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Reached")
-                       .min(0)
-                       .max(1000)
-                       .value(val.thresholdPositionReached)
-                       .decimals(3)
-                   )
-                   .addChild(RemoteGui::makeTextLabel("Max vel:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_Max_vel_pos")
-                       .min(0)
-                       .max(1000)
-                       .value(val.maxPosVel)
-                       .decimals(3)
-                   )
-                   .addChild(RemoteGui::makeTextLabel("KP:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_KP_pos")
-                       .min(0)
-                       .max(10)
-                       .value(val.kpPos)
-                       .decimals(3)
-                   )
-                   .addChild(new RemoteGui::HSpacer)
-
-                   .addChild(RemoteGui::makeTextLabel("Orientation:"))
-                   .addChild(RemoteGui::makeTextLabel("Near:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Near")
-                       .min(0)
-                       .max(3.14f)
-                       .value(val.thresholdOrientationNear)
-                       .decimals(3)
-                   )
-                   .addChild(RemoteGui::makeTextLabel("Reached:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Reached")
-                       .min(0)
-                       .max(3.14f)
-                       .value(val.thresholdOrientationReached)
-                       .decimals(3)
-                   )
-                   .addChild(RemoteGui::makeTextLabel("Max vel:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_Max_vel_ori")
-                       .min(0)
-                       .max(31.4f)
-                       .value(val.maxOriVel)
-                       .decimals(3)
-                   )
-                   .addChild(RemoteGui::makeTextLabel("KP:"))
-                   .addChild(
-                       RemoteGui::makeFloatSpinBox(name + "_KP_ori")
-                       .min(0)
-                       .max(10)
-                       .value(val.kpOri)
-                       .decimals(3)
-                   )
-                   .addChild(new RemoteGui::HSpacer)
-               );
-    }
-
-    void getValueFromMap(CartesianWaypointControllerConfig& cfg,
-                         RemoteGui::ValueMap const& values, std::string const& name)
-    {
-        getValueFromMap(cfg.maxPositionAcceleration, values, name + "_maxAcc_Pos");
-        getValueFromMap(cfg.maxOrientationAcceleration, values, name + "_maxAcc_Ori");
-        getValueFromMap(cfg.maxNullspaceAcceleration, values, name + "_maxAcc_Null");
-
-        getValueFromMap(cfg.kpJointLimitAvoidance, values, name + "_JointLimitAvoidance_KP");
-        getValueFromMap(cfg.jointLimitAvoidanceScale, values, name + "_JointLimitAvoidance_Scale");
-
-        getValueFromMap(cfg.thresholdOrientationNear, values, name + "_Thresholds_Ori_Near");
-        getValueFromMap(cfg.thresholdOrientationReached, values, name + "_Thresholds_Ori_Reached");
-        getValueFromMap(cfg.thresholdPositionNear, values, name + "_Thresholds_Pos_Near");
-        getValueFromMap(cfg.thresholdPositionReached, values, name + "_Thresholds_Pos_Reached");
-
-        getValueFromMap(cfg.maxOriVel, values, name + "_Max_vel_ori");
-        getValueFromMap(cfg.maxPosVel, values, name + "_Max_vel_pos");
-        getValueFromMap(cfg.kpOri, values, name + "_KP_ori");
-        getValueFromMap(cfg.kpPos, values, name + "_KP_pos");
-    }
-}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
deleted file mode 100644
index 238ba2ae38f1f20aeba20604c62c86f1e432488b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2019
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#pragma once
-
-#include <ArmarXGui/libraries/RemoteGui/RemoteGui.h>
-#include <RobotAPI/interface/core/CartesianWaypointControllerConfig.h>
-
-//make
-namespace armarx::RemoteGui
-{
-    detail::GroupBoxBuilder makeConfigGui(
-        const std::string& name,
-        const CartesianWaypointControllerConfig& val);
-
-    void getValueFromMap(CartesianWaypointControllerConfig& cfg,
-                         RemoteGui::ValueMap const& values, std::string const& name);
-}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
deleted file mode 100644
index dc6aadb5ad0b72d8ebe19ad8e8f1b21b3a8b7d9b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2019
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "RemoteGui.h"
-namespace armarx::RemoteGui
-{
-    detail::GroupBoxBuilder makeConfigGui(
-        const std::string& name,
-        const NJointCartesianWaypointControllerRuntimeConfig& val)
-    {
-        detail::GroupBoxBuilder builder = makeConfigGui(name, val.wpCfg);
-        auto& cs = builder.child(0)->children;
-
-        cs.emplace_back(RemoteGui::makeTextLabel("Force limit:"));
-        cs.emplace_back(RemoteGui::makeFloatSpinBox(name + "_forceThreshold")
-                        .min(-1)
-                        .max(1000)
-                        .value(val.forceThreshold)
-                        .decimals(3));
-        cs.emplace_back(RemoteGui::makeCheckBox(name + "_forceThresholdInRobotRootZ")
-                        .value(val.forceThresholdInRobotRootZ)
-                        .label("Threshold only in root z"));
-
-
-        cs.emplace_back(new RemoteGui::Widget);
-        cs.emplace_back(RemoteGui::makeCheckBox(name + "_optimizeNullspaceIfTargetWasReached")
-                        .value(val.optimizeNullspaceIfTargetWasReached)
-                        .label("Optimize nullspace if target was reached"));
-        cs.emplace_back(new RemoteGui::HSpacer);
-
-        return builder;
-    }
-
-    void getValueFromMap(NJointCartesianWaypointControllerRuntimeConfig& cfg,
-                         RemoteGui::ValueMap const& values, std::string const& name)
-    {
-        getValueFromMap(cfg.wpCfg,          values, name);
-        getValueFromMap(cfg.forceThreshold, values, name + "_forceThreshold");
-        getValueFromMap(cfg.forceThresholdInRobotRootZ, values, name + "_forceThresholdInRobotRootZ");
-        getValueFromMap(cfg.optimizeNullspaceIfTargetWasReached, values, name + "_optimizeNullspaceIfTargetWasReached");
-    }
-}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
deleted file mode 100644
index f8b06a1a00c52963ff590e196ba95c97bdb3cefb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2019
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#pragma once
-
-#include "../CartesianWaypointControllerConfig/RemoteGui.h"
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
-
-//make
-namespace armarx::RemoteGui
-{
-    detail::GroupBoxBuilder makeConfigGui(
-        const std::string& name,
-        const NJointCartesianWaypointControllerRuntimeConfig& val);
-
-
-    void getValueFromMap(NJointCartesianWaypointControllerRuntimeConfig& cfg,
-                         RemoteGui::ValueMap const& values, std::string const& name);
-}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/test/CMakeLists.txt b/source/RobotAPI/libraries/ControllerUIUtility/test/CMakeLists.txt
deleted file mode 100644
index d1ed61d5cf4e1b5e2da1b3b01f95da983e170772..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore RobotAPIControllerUIUtility)
- 
-armarx_add_test(RobotAPIControllerUIUtilityTest ControllerUIUtilityTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/test/ControllerUIUtilityTest.cpp b/source/RobotAPI/libraries/ControllerUIUtility/test/ControllerUIUtilityTest.cpp
deleted file mode 100644
index bcaf923176aed0de80eaca65acd20c0deef6a001..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/test/ControllerUIUtilityTest.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2019
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::ControllerUIUtility
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include "../CartesianWaypointControllerConfig/RemoteGui.h"
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
-    BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/DMPController/CMakeLists.txt b/source/RobotAPI/libraries/DMPController/CMakeLists.txt
deleted file mode 100644
index 1bf626ee812b91c2120ea7cbd1ad40bbbf00a716..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/CMakeLists.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-set(LIB_NAME       DMPController)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(DMP QUIET)
-find_package(IVT QUIET)
-find_package(MMMCore QUIET)
-find_package(MMMTools QUIET)
-
-armarx_build_if(DMP_FOUND "DMP not available")
-
-
-set(LIBS
-    ArmarXCoreObservers
-    ArmarXCoreStatechart
-    ArmarXCoreEigen3Variants
-    VirtualRobot
-    RobotUnit
-    ${DMP_LIBRARIES}
-    )
-
-set(LIB_FILES   TaskSpaceDMPController.cpp)
-set(LIB_HEADERS TaskSpaceDMPController.h)
-
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-if (DMP_FOUND)
-    target_include_directories("${LIB_NAME}" PUBLIC ${DMP_INCLUDE_DIRS})
-endif()
-
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
deleted file mode 100644
index e0909b0b4c22f1f4617b9bd89cfe81e7258a499a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ /dev/null
@@ -1,473 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::DMPController
- * @author     zhou ( you dot zhou at kit dot edu )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "TaskSpaceDMPController.h"
-
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <boost/archive/xml_oarchive.hpp>
-#include <boost/archive/xml_iarchive.hpp>
-#include <boost/archive/text_iarchive.hpp>
-#include <boost/archive/text_oarchive.hpp>
-
-
-using namespace armarx;
-
-
-
-void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
-{
-    canVal = flow(canVal, deltaT, currentPose, twist);
-}
-
-double TaskSpaceDMPController::flow(double canVal, double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
-{
-    if (paused)
-    {
-        targetVel.setZero();
-        return canVal;
-    }
-    if (canVal < 0.1 && config.DMPStyle == "Periodic")
-    {
-        canVal = config.motionTimeDuration;
-    }
-    if (canVal < 1e-8 && config.DMPStyle == "Discrete")
-    {
-        targetVel.setZero();
-        return canVal;
-    }
-
-    Eigen::Vector3f currentPosition;
-    Eigen::Quaterniond currentOrientation;
-    double posiError = 0;
-    double oriError = 0;
-
-    getError(currentPose, currentPosition, currentOrientation, posiError, oriError);
-
-    double poseError = posiError + config.phaseStopParas.mm2radi * oriError;
-
-    double phaseDist;
-    if (isDisturbance)
-    {
-        phaseDist = config.phaseStopParas.backDist;
-    }
-    else
-    {
-        phaseDist = config.phaseStopParas.goDist;
-    }
-    double phaseL = config.phaseStopParas.maxValue;
-    double phaseK = config.phaseStopParas.slop;
-
-    double phaseStop = phaseL / (1 + exp(-phaseK * (poseError - phaseDist)));
-    double mpcFactor = 1 - (phaseStop / phaseL);
-
-    if (mpcFactor < 0.1)
-    {
-        isDisturbance = true;
-    }
-
-    if (mpcFactor > 0.9)
-    {
-        isDisturbance = false;
-    }
-
-    double timeDuration = config.motionTimeDuration;
-    canVal -= tau * deltaT * 1;// / (1 + phaseStop) ;
-
-
-    DMP::Vec<DMP::DMPState > temporalState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
-
-    // scale translation velocity
-    for (size_t i = 0; i < 3; ++i)
-    {
-        currentState[i].vel = tau * temporalState[i].vel * config.DMPAmplitude / timeDuration;
-        currentState[i].pos += deltaT * currentState[i].vel;
-    }
-
-    // define the translation velocity
-    if (isPhaseStopControl)
-    {
-        float vel0, vel1;
-
-        Eigen::Vector3f linearVel;
-        linearVel << twist(0), twist(1), twist(2);
-        for (size_t i = 0; i < 3; i++)
-        {
-            vel0 = currentState[i].vel;
-            vel1 = config.phaseStopParas.Kpos * (targetPoseVec[i] - currentPosition(i)) - config.phaseStopParas.Dpos * linearVel(i);
-            targetVel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
-        }
-    }
-    else
-    {
-        for (size_t i = 0; i < 3; i++)
-        {
-            targetVel(i) = currentState[i].vel;
-        }
-    }
-
-
-
-    // define the rotation velocity
-    Eigen::Quaterniond dmpQuaternionVel;
-    dmpQuaternionVel.w() = temporalState[3].vel;
-    dmpQuaternionVel.x() = temporalState[4].vel;
-    dmpQuaternionVel.y() = temporalState[5].vel;
-    dmpQuaternionVel.z() = temporalState[6].vel;
-
-    Eigen::Quaterniond dmpQuaternionPosi;
-    dmpQuaternionPosi.w() = currentState[3].pos;
-    dmpQuaternionPosi.x() = currentState[4].pos;
-    dmpQuaternionPosi.y() = currentState[5].pos;
-    dmpQuaternionPosi.z() = currentState[6].pos;
-
-
-    Eigen::Quaterniond angularVel0 = dmpQuaternionVel * dmpQuaternionPosi.inverse();
-    angularVel0.w() *= 2;
-    angularVel0.x() *= 2;
-    angularVel0.y() *= 2;
-    angularVel0.z() *= 2;
-
-
-    double angularChange =  angularVel0.angularDistance(oldDMPAngularVelocity);
-    if (angularVel0.w() * oldDMPAngularVelocity.w() < 0 &&
-        angularVel0.x() * oldDMPAngularVelocity.x() < 0 &&
-        angularVel0.y() * oldDMPAngularVelocity.y() < 0 &&
-        angularVel0.z() * oldDMPAngularVelocity.z() < 0 &&
-        angularChange < 1e-2)
-    {
-        angularVel0.w() = - angularVel0.w();
-        angularVel0.x() = - angularVel0.x();
-        angularVel0.y() = - angularVel0.y();
-        angularVel0.z() = - angularVel0.z();
-    }
-    oldDMPAngularVelocity = angularVel0;
-
-    // scale orientation velocity
-    angularVel0.w() = 0;
-    angularVel0.x() = tau * angularVel0.x() * config.oriAmplitude / timeDuration;
-    angularVel0.y() = tau * angularVel0.y() * config.oriAmplitude / timeDuration;
-    angularVel0.z() = tau * angularVel0.z() * config.oriAmplitude / timeDuration;
-
-    //    Eigen::Quaterniond scaledQuat = (angularVel0 * dmpQuaternionPosi);
-    //    currentState[3].vel = 0.5 * scaledQuat.w();
-    //    currentState[4].vel = 0.5 * scaledQuat.x();
-    //    currentState[6].vel = 0.5 * scaledQuat.z();
-    //    currentState[5].vel = 0.5 * scaledQuat.y();
-
-    for (size_t i = 3; i < 7; ++i)
-    {
-        currentState[i].vel = tau * temporalState[i].vel * config.oriAmplitude / timeDuration;
-        currentState[i].pos += currentState[i].vel * deltaT;
-    }
-
-    if (isPhaseStopControl)
-    {
-        Eigen::Vector3f currentAngularVel;
-        currentAngularVel << twist(3), twist(4), twist(5);
-
-        Eigen::Quaternionf targetQuaternionf;
-        targetQuaternionf.w() = targetPoseVec[3];
-        targetQuaternionf.x() = targetPoseVec[4];
-        targetQuaternionf.y() = targetPoseVec[5];
-        targetQuaternionf.z() = targetPoseVec[6];
-
-        Eigen::Matrix3f desiredMat(targetQuaternionf);
-        Eigen::Matrix3f currentMat = currentPose.block<3, 3>(0, 0);
-        Eigen::Matrix3f diffMat = desiredMat * currentMat.inverse();
-        Eigen::Vector3f diffRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-        Eigen::Vector3f angularVel1 = config.phaseStopParas.Kori * diffRPY - config.phaseStopParas.Dori * currentAngularVel;
-
-        targetVel(3) = mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
-        targetVel(4) = mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
-        targetVel(5) = mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1(2);
-    }
-    else
-    {
-        targetVel(3) = angularVel0.x() ;
-        targetVel(4) = angularVel0.y();
-        targetVel(5) = angularVel0.z();
-    }
-
-    debugData.canVal = canVal;
-    debugData.oriError = oriError;
-    debugData.posiError = posiError;
-    debugData.mpcFactor = mpcFactor;
-    debugData.poseError = poseError;
-
-    return canVal;
-}
-
-void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios)
-{
-    if (ratios.size() != fileNames.size())
-    {
-        ARMARX_ERROR << "ratios should have the same size with files";
-        return;
-    }
-
-
-    DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
-    double ratioSum = 0;
-    for (size_t i = 0; i < fileNames.size(); ++i)
-    {
-        DMP::SampledTrajectoryV2 traj;
-        std::string absPath;
-        ArmarXDataPath::getAbsolutePath(fileNames.at(i), absPath);
-        traj.readFromCSVFile(absPath);
-        traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
-        trajs.push_back(traj);
-
-        ratioSum += ratios.at(i);
-    }
-
-    if (ratioSum == 0)
-    {
-        ARMARX_ERROR << "ratios are invalid. The sum is equal to 0";
-        return;
-    }
-
-    DMP::DVec ratiosVec;
-    ratiosVec.resize(ratios.size());
-    for (size_t i = 0; i < ratios.size(); ++i)
-    {
-        ratiosVec.at(i) = ratios.at(i) / ratioSum;
-    }
-
-    dmpPtr->learnFromTrajectories(trajs);
-    dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratiosVec);
-}
-
-void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames)
-{
-    std::vector<double> ratios;
-    for (size_t i = 0; i < fileNames.size(); ++i)
-    {
-        if (i == 0)
-        {
-            ratios.push_back(1.0);
-        }
-        else
-        {
-            ratios.push_back(0.0);
-        }
-    }
-
-    learnDMPFromFiles(fileNames, ratios);
-}
-
-void TaskSpaceDMPController::learnDMPFromSampledTrajectory(const DMP::Vec<DMP::SampledTrajectoryV2>& trajs, const std::vector<double>& ratios)
-{
-    dmpPtr->learnFromTrajectories(trajs);
-    dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
-}
-
-void TaskSpaceDMPController::setRatios(const std::vector<double>& ratios)
-{
-    dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
-}
-
-void TaskSpaceDMPController::learnDMPFromTrajectory(const TrajectoryPtr& traj)
-{
-    ARMARX_CHECK_EQUAL(traj->dim(), 7);
-    DMP::SampledTrajectoryV2 dmpTraj;
-
-    DMP::DVec timestamps(traj->getTimestamps());
-    for (size_t i = 0; i < traj->dim(); ++i)
-    {
-        DMP::DVec dimData(traj->getDimensionData(i, 0));
-        dmpTraj.addDimension(timestamps, dimData);
-    }
-
-    DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
-    dmpTraj = DMP::SampledTrajectoryV2::normalizeTimestamps(dmpTraj, 0, 1);
-    trajs.push_back(dmpTraj);
-    DMP::DVec ratiosVec;
-    ratiosVec.push_back(1.0);
-    dmpPtr->learnFromTrajectories(trajs);
-    dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratiosVec);
-}
-
-void TaskSpaceDMPController::loadDMPFromString(const std::string& dmpStr)
-{
-    std::stringstream ss;
-    ss.str(dmpStr);
-    boost::archive::text_iarchive ia{ss};
-    DMP::UMITSMP* dmpPointer;
-    ia >> dmpPointer;
-    dmpPtr.reset(dmpPointer);
-}
-
-
-std::string TaskSpaceDMPController::saveDMPToString()
-{
-    std::stringstream ss;
-    boost::archive::text_oarchive oa{ss};
-    oa << dmpPtr.get();
-    return ss.str();
-}
-
-void TaskSpaceDMPController::loadDMPFromXML(const std::string& dmpXML)
-{
-    std::string absPath;
-    ArmarXDataPath::getAbsolutePath(dmpXML, absPath);
-    DMP::UMITSMP* dmpPointer;
-    std::ifstream ifs(absPath);
-    boost::archive::xml_iarchive ai(ifs);
-    ai >>  boost::serialization::make_nvp("dmp", dmpPointer);
-    dmpPtr.reset(dmpPointer);
-}
-
-
-void TaskSpaceDMPController::saveDMPToXML(const std::string& dmpXML)
-{
-    std::string absPath;
-    ArmarXDataPath::getAbsolutePath(dmpXML, absPath);
-    std::ofstream ofs(absPath);
-    boost::archive::xml_oarchive ar(ofs);
-    DMP::UMITSMP* dmpPointer = dmpPtr.get();
-    ar << boost::serialization::make_nvp("dmp", dmpPointer);
-    dmpPtr.reset(dmpPointer);
-}
-
-void TaskSpaceDMPController::setViaPose(double canVal, const Eigen::Matrix4f& viaPose)
-{
-
-    setViaPose(canVal, eigen4f2vec(viaPose));
-}
-
-void TaskSpaceDMPController::setViaPose(double canVal, const std::vector<double>& viaPoseWithQuaternion)
-{
-    if (canVal <= dmpPtr->getUMin())
-    {
-        goalPoseVec = viaPoseWithQuaternion;
-    }
-    dmpPtr->setViaPoint(canVal, viaPoseWithQuaternion);
-}
-
-void TaskSpaceDMPController::removeAllViaPoints()
-{
-    dmpPtr->removeViaPoints();
-}
-
-void TaskSpaceDMPController::prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose)
-{
-    std::vector<double> currentPoseVec = eigen4f2vec(currentPose);
-    std::vector<double> goalPoseVec = eigen4f2vec(goalPose);
-
-    prepareExecution(currentPoseVec, goalPoseVec);
-}
-
-void TaskSpaceDMPController::prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec)
-{
-    ARMARX_CHECK_EQUAL(currentPoseVec.size(), 7);
-    ARMARX_CHECK_EQUAL(goalPoseVec.size(), 7);
-
-    ARMARX_IMPORTANT << "prepareExecution: currentPoseVec: " << currentPoseVec;
-    for (size_t i = 0; i < currentPoseVec.size(); ++i)
-    {
-        currentState[i].pos = currentPoseVec.at(i);
-        currentState[i].vel = 0;
-        targetPoseVec.at(i) = currentPoseVec.at(i);
-    }
-
-    dmpPtr->prepareExecution(goalPoseVec, currentState, 1,  1);
-    this->goalPoseVec = goalPoseVec;
-    isDisturbance = false;
-    canVal = config.motionTimeDuration;
-    oldDMPAngularVelocity.setIdentity();
-
-}
-
-void TaskSpaceDMPController::setSpeed(double times)
-{
-    if (times <= 0)
-    {
-        ARMARX_WARNING << "TaskSpaceDMPController: cannot set non-positive speed times";
-    }
-
-    tau = times;
-}
-
-void TaskSpaceDMPController::setAmplitude(double amp)
-{
-    if (amp <= 0)
-    {
-        ARMARX_WARNING << "TaskSpaceDMPController: cannot set non-positive amplitude";
-    }
-    config.DMPAmplitude = amp;
-}
-
-std::vector<double> TaskSpaceDMPController::eigen4f2vec(const Eigen::Matrix4f& pose)
-{
-    std::vector<double> viaPoseVec;
-    viaPoseVec.resize(7);
-
-    for (size_t i = 0; i < 3; ++i)
-    {
-        viaPoseVec.at(i) = pose(i, 3);
-    }
-
-    VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(pose);
-
-    viaPoseVec.at(3) = quat.w;
-    viaPoseVec.at(4) = quat.x;
-    viaPoseVec.at(5) = quat.y;
-    viaPoseVec.at(6) = quat.z;
-
-    return viaPoseVec;
-}
-
-void TaskSpaceDMPController::getError(const Eigen::Matrix4f& currentPose, Eigen::Vector3f& currentPosition, Eigen::Quaterniond& currentOrientation, double& posiError, double& oriError)
-{
-    currentPosition.setZero();
-    currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
-
-    VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
-    currentOrientation.w() = quat.w;
-    currentOrientation.x() = quat.x;
-    currentOrientation.y() = quat.y;
-    currentOrientation.z() = quat.z;
-
-    posiError = 0;
-    for (size_t i = 0; i < 3; ++i)
-    {
-        posiError += pow(currentPosition(i) - targetPoseVec[i], 2);
-    }
-    posiError = sqrt(posiError);
-
-    Eigen::Quaterniond targetQuaternion;
-    targetQuaternion.w() = targetPoseVec[3];
-    targetQuaternion.x() = targetPoseVec[4];
-    targetQuaternion.y() = targetPoseVec[5];
-    targetQuaternion.z() = targetPoseVec[6];
-
-    oriError = targetQuaternion.angularDistance(currentOrientation);
-    if (oriError > M_PI)
-    {
-        oriError = 2 * M_PI - oriError;
-    }
-
-}
-
-
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
deleted file mode 100644
index 169e70b3fef99bf4dbcdb70161e11e45b6e98327..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ /dev/null
@@ -1,288 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::DMPController
- * @author     zhou ( you dot zhou at kit dot edu )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#ifndef _ARMARX_LIB_RobotAPI_TaskSpaceDMPController_H
-#define _ARMARX_LIB_RobotAPI_TaskSpaceDMPController_H
-
-
-#include <dmp/representation/dmp/umitsmp.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/MathTools.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-
-namespace armarx
-{
-
-    struct PhaseStopParams
-    {
-        float goDist = 80;
-        float backDist = 50;
-        float maxValue = 100;
-        float slop = 1000;
-        float Kpos = 1;
-        float Dpos = 2;
-        float Kori = 1;
-        float Dori = 0;
-        float mm2radi = 10;
-    };
-
-    struct TaskSpaceDMPControllerConfig
-    {
-        int DMPKernelSize = 50;
-        std::string DMPMode = "Linear";
-        std::string DMPStyle = "Discrete";
-        float DMPAmplitude = 1;
-        float oriAmplitude = 1;
-        float motionTimeDuration = 10;
-        PhaseStopParams phaseStopParas;
-    };
-
-    struct DebugInfo
-    {
-        double canVal;
-        double mpcFactor;
-        double poseError;
-        double posiError;
-        double oriError;
-    };
-
-    /**
-    * @defgroup Library-TaskSpaceDMPController TaskSpaceDMPController
-    * @ingroup Library-RobotUnit-NJointControllers
-    * A description of the library TaskSpaceDMPController.
-    *
-    * @class TaskSpaceDMPController
-    * @ingroup Library-TaskSpaceDMPController
-    * @brief Brief description of class TaskSpaceDMPController.
-    *
-    * Detailed description of class TaskSpaceDMPController.
-    */
-    class TaskSpaceDMPController
-    {
-    public:
-        TaskSpaceDMPController(std::string name, const TaskSpaceDMPControllerConfig& config, bool isPhaseStopControl = true)
-        {
-            this->config = config;
-
-            int ModeInd;
-            if (config.DMPMode == "MinimumJerk")
-            {
-                ModeInd = 2;
-            }
-            else
-            {
-                ModeInd = 1;
-            }
-
-
-            dmpPtr.reset(new DMP::UMITSMP(config.DMPKernelSize, ModeInd));
-            canVal = config.motionTimeDuration;
-
-            targetPoseVec.resize(7);
-            targetVel.resize(6);
-            targetVel.setZero();
-            currentState.resize(7);
-
-            this->isPhaseStopControl = isPhaseStopControl;
-            dmpName = name;
-            this->paused = false;
-            tau = 1;
-        }
-
-        std::string getName()
-        {
-            return dmpName;
-        }
-
-
-        void flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist);
-        double flow(double canVal, double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist);
-
-        Eigen::VectorXf getTargetVelocity()
-        {
-            return targetVel;
-        }
-
-        std::vector<double> getTargetPose()
-        {
-            return targetPoseVec;
-        }
-
-        Eigen::Matrix4f getTargetPoseMat()
-        {
-            Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(targetPoseVec.at(4), targetPoseVec.at(5), targetPoseVec.at(6), targetPoseVec.at(3));
-            res(0, 3) = targetPoseVec.at(0);
-            res(1, 3) = targetPoseVec.at(1);
-            res(2, 3) = targetPoseVec.at(2);
-
-            return res;
-        }
-
-        Eigen::Matrix4f getIntegratedPoseMat()
-        {
-            Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(currentState.at(4).pos,
-                                  currentState.at(5).pos,
-                                  currentState.at(6).pos,
-                                  currentState.at(3).pos);
-            res(0, 3) = currentState.at(0).pos;
-            res(1, 3) = currentState.at(1).pos;
-            res(2, 3) = currentState.at(2).pos;
-
-            return res;
-        }
-
-        void learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios);
-        void learnDMPFromFiles(const std::vector<std::string>& fileNames);
-
-        void learnDMPFromSampledTrajectory(const DMP::Vec<DMP::SampledTrajectoryV2 >& trajs);
-        void learnDMPFromTrajectory(const TrajectoryPtr& traj);
-
-        void loadDMPFromXML(const std::string& dmpXML);
-        void loadDMPFromString(const std::string& dmpStr);
-
-        void saveDMPToXML(const std::string& dmpXML);
-        std::string saveDMPToString();
-
-        void setViaPose(double canVal, const Eigen::Matrix4f& viaPose);
-        void setViaPose(double canVal, const std::vector<double>& viaPoseWithQuaternion);
-
-        void removeAllViaPoints();
-        void prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose);
-        void prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec);
-
-        void setSpeed(double times);
-        void setAmplitude(double amp);
-
-        void setGoalPose(const Eigen::Matrix4f& goalPose)
-        {
-            setViaPose(dmpPtr->getUMin(), goalPose);
-        }
-
-        void setGoalPoseVec(const std::vector<double> goalPoseVec)
-        {
-            setViaPose(dmpPtr->getUMin(), goalPoseVec);
-        }
-
-        void learnDMPFromSampledTrajectory(const DMP::Vec<DMP::SampledTrajectoryV2>& trajs, const std::vector<double>& ratios);
-        void setRatios(const std::vector<double>& ratios);
-
-        DebugInfo debugData;
-        std::vector<double> eigen4f2vec(const Eigen::Matrix4f& pose);
-
-        DMP::UMITSMPPtr getDMP()
-        {
-            return dmpPtr;
-        }
-
-        void pauseController()
-        {
-            this->paused = true;
-        }
-        void resumeController()
-        {
-            this->paused = false;
-        }
-
-        void setWeights(const std::vector<std::vector<double> >& weights)
-        {
-            dmpPtr->setWeights(weights);
-        }
-
-        void setTranslWeights(const std::vector<std::vector<double> >& weights)
-        {
-            ARMARX_CHECK_EQUAL(weights.size(), 3);
-
-            for (size_t i = 0; i < 3; ++i)
-            {
-                dmpPtr->setWeights(i, weights[i]);
-            }
-        }
-
-        void setRotWeights(const std::vector<std::vector<double> >& weights)
-        {
-            ARMARX_CHECK_EQUAL(weights.size(), 4);
-
-            for (size_t i = 0; i < 4; ++i)
-            {
-                dmpPtr->setWeights(3 + i, weights[i]);
-            }
-        }
-
-        DMP::DVec2d getWeights()
-        {
-            return dmpPtr->getWeights();
-        }
-
-        DMP::DVec2d getTranslWeights()
-        {
-            DMP::DVec2d res;
-            DMP::DVec2d weights = getWeights();
-            for (size_t i = 0; i < 3; ++i)
-            {
-                res.push_back(weights[i]);
-            }
-            return res;
-        }
-
-        DMP::DVec2d getRotWeights()
-        {
-            DMP::DVec2d res;
-            DMP::DVec2d weights = getWeights();
-            for (size_t i = 3; i < 7; ++i)
-            {
-                res.push_back(weights[i]);
-            }
-            return res;
-        }
-
-        double canVal;
-        bool isPhaseStopControl;
-        std::string dmpName;
-        DMP::UMITSMPPtr dmpPtr;
-        TaskSpaceDMPControllerConfig config;
-    private:
-
-        double tau;
-        DMP::DVec goalPoseVec;
-
-        Eigen::VectorXf targetVel;
-        DMP::DVec targetPoseVec;
-
-        DMP::Vec<DMP::DMPState > currentState;
-        bool paused;
-
-
-        bool isDisturbance;
-
-
-        void getError(const Eigen::Matrix4f& pose, Eigen::Vector3f& position, Eigen::Quaterniond& quaternion, double& posiError, double& oriError);
-
-        Eigen::Quaterniond oldDMPAngularVelocity;
-    };
-
-    using TaskSpaceDMPControllerPtr = std::shared_ptr<TaskSpaceDMPController>;
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/DMPController/test/CMakeLists.txt b/source/RobotAPI/libraries/DMPController/test/CMakeLists.txt
deleted file mode 100644
index df7b127d74b14e3a3b6bce5c3fd556324c3dfa73..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore DMPController)
- 
-armarx_add_test(DMPControllerTest DMPControllerTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/DMPController/test/DMPControllerTest.cpp b/source/RobotAPI/libraries/DMPController/test/DMPControllerTest.cpp
deleted file mode 100644
index ce13dde7e8ae02eeeade7e2be1e52fce411acc34..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/test/DMPControllerTest.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::DMPController
- * @author     zhou ( you dot zhou at kit dot edu )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::DMPController
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include "../TaskSpaceDMPController.h"
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
-    BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/DSControllers/CMakeLists.txt b/source/RobotAPI/libraries/DSControllers/CMakeLists.txt
deleted file mode 100644
index 317a93a88917c1bc601c450c07e4f105a0b2a07e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/CMakeLists.txt
+++ /dev/null
@@ -1,56 +0,0 @@
-set(LIB_NAME       DSControllers)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(Eigen3 QUIET)
-find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
-find_package(MATHLIB QUIET)
-
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-armarx_build_if(MATHLIB_FOUND "MATHLIB not available")
-
-if (Eigen3_FOUND AND Simox_FOUND AND MATHLIB_FOUND)
-    include_directories(${Simox_INCLUDE_DIRS})
-    include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
-    include_directories(${MATHLIB_INCLUDE_DIRS})
-endif()
-
-
-message(STATUS "mathlib is ${MATHLIB_LIBS}")
-
-set(LIBS ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
-    VirtualRobot
-    Saba
-    SimDynamics
-    RobotUnit
-    RobotAPIUnits
-    RobotAPICore
-    RobotAPIInterfaces
-    ${MATHLIB_LIB}
-)
-
-
-
-set(LIB_FILES
-./DSRTBimanualController.cpp
-./DSJointCarryController.cpp
-./DSRTController.cpp
-./GMRDynamics.cpp
-./Gaussians.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(LIB_HEADERS
-./DSRTBimanualController.h
-./DSJointCarryController.h
-./DSRTController.h
-./GMRDynamics.h
-./Gaussians.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
-
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-# add unit tests
diff --git a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
deleted file mode 100644
index d85b1a78d4bd192e56e95c1c5eef0ed1350138c2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
+++ /dev/null
@@ -1,719 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    DSController::ArmarXObjects::DSJointCarryController
- * @author     Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "DSJointCarryController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-using namespace armarx;
-
-NJointControllerRegistration<DSJointCarryController> registrationControllerDSJointCarryController("DSJointCarryController");
-
-void DSJointCarryController::onInitNJointController()
-{
-    ARMARX_INFO << "init ...";
-    controllerStopRequested = false;
-    controllerRunFinished = false;
-    runTask("DSJointCarryControllerTask", [&]
-    {
-        CycleUtil c(1);
-        getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-        while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
-        {
-            ARMARX_INFO << deactivateSpam(1) << "control fct";
-            if (isControllerActive())
-            {
-                controllerRun();
-            }
-            c.waitForCycleDuration();
-        }
-        controllerRunFinished = true;
-        ARMARX_INFO << "Control Fct finished";
-    });
-
-
-}
-
-void DSJointCarryController::onDisconnectNJointController()
-{
-    ARMARX_INFO << "disconnect";
-    controllerStopRequested = true;
-    while (!controllerRunFinished)
-    {
-        ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
-        usleep(10000);
-    }
-}
-
-
-DSJointCarryController::DSJointCarryController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-{
-    cfg = DSJointCarryControllerConfigPtr::dynamicCast(config);
-    useSynchronizedRtRobot();
-
-    VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
-    VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
-
-    left_jointNames.clear();
-    right_jointNames.clear();
-
-    ARMARX_CHECK_EXPRESSION(left_ns) << "LeftArm";
-    ARMARX_CHECK_EXPRESSION(right_ns) << "RightArm";
-
-    // for left arm
-    for (size_t i = 0; i < left_ns->getSize(); ++i)
-    {
-        std::string jointName = left_ns->getNode(i)->getName();
-        left_jointNames.push_back(jointName);
-        ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-        ARMARX_CHECK_EXPRESSION(ct);
-        const SensorValueBase* sv = useSensorValue(jointName);
-        ARMARX_CHECK_EXPRESSION(sv);
-        auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
-        ARMARX_CHECK_EXPRESSION(casted_ct);
-        left_torque_targets.push_back(casted_ct);
-
-        const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
-        const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-        const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
-        const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-        if (!torqueSensor)
-        {
-            ARMARX_WARNING << "No Torque sensor available for " << jointName;
-        }
-        if (!gravityTorqueSensor)
-        {
-            ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
-        }
-
-        left_torqueSensors.push_back(torqueSensor);
-        left_gravityTorqueSensors.push_back(gravityTorqueSensor);
-        left_velocitySensors.push_back(velocitySensor);
-        left_positionSensors.push_back(positionSensor);
-    };
-
-    // for right arm
-    for (size_t i = 0; i < right_ns->getSize(); ++i)
-    {
-        std::string jointName = right_ns->getNode(i)->getName();
-        right_jointNames.push_back(jointName);
-        ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-        ARMARX_CHECK_EXPRESSION(ct);
-        const SensorValueBase* sv = useSensorValue(jointName);
-        ARMARX_CHECK_EXPRESSION(sv);
-        auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
-        ARMARX_CHECK_EXPRESSION(casted_ct);
-        right_torque_targets.push_back(casted_ct);
-
-        const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
-        const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-        const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
-        const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-        if (!torqueSensor)
-        {
-            ARMARX_WARNING << "No Torque sensor available for " << jointName;
-        }
-        if (!gravityTorqueSensor)
-        {
-            ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
-        }
-
-        right_torqueSensors.push_back(torqueSensor);
-        right_gravityTorqueSensors.push_back(gravityTorqueSensor);
-        right_velocitySensors.push_back(velocitySensor);
-        right_positionSensors.push_back(positionSensor);
-    };
-
-
-    const SensorValueBase* svlf = useSensorValue("FT L");
-    leftForceTorque = svlf->asA<SensorValueForceTorque>();
-    const SensorValueBase* svrf = useSensorValue("FT R");
-    rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
-    ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
-    ARMARX_INFO << "Initialized " << right_torque_targets.size() << " targets for the right arm";
-
-    left_arm_tcp =  left_ns->getTCP();
-    right_arm_tcp =  right_ns->getTCP();
-
-    left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL8_Wri2");
-    right_sensor_frame  = right_ns->getRobot()->getRobotNode("ArmR8_Wri2");
-
-    left_ik.reset(new VirtualRobot::DifferentialIK(left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-    right_ik.reset(new VirtualRobot::DifferentialIK(right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-    // ?? not sure about this
-    DSJointCarryControllerSensorData initSensorData;
-    initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
-    initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
-    initSensorData.left_force.setZero();
-    initSensorData.right_force.setZero();
-    initSensorData.currentTime = 0;
-    controllerSensorData.reinitAllBuffers(initSensorData);
-
-    std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
-    ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
-
-    std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
-    ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
-
-    left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
-    left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
-    left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
-    left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
-    right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
-    right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
-    right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
-    right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
-
-
-    // set initial joint velocities filter
-    left_jointVelocity_filtered.resize(left_torque_targets.size());
-    left_jointVelocity_filtered.setZero();
-    right_jointVelocity_filtered.resize(left_torque_targets.size());
-    right_jointVelocity_filtered.setZero();
-
-    // do we need to duplicate this?
-    DSJointCarryControllerControlData initData;
-    for (size_t i = 0; i < 3; ++i)
-    {
-        initData.leftDesiredLinearVelocity(i) = 0;
-        initData.leftDesiredAngularError(i) = 0;
-        initData.rightDesiredLinearVelocity(i) = 0;
-        initData.rightDesiredAngularError(i) = 0;
-
-    }
-    reinitTripleBuffer(initData);
-
-    Ctrl2InterfaceData initCtrl2InterfaceData;
-    initCtrl2InterfaceData.guardZVel = 0;
-    ctrl2InterfaceData.reinitAllBuffers(initCtrl2InterfaceData);
-
-    Interface2CtrlData initInterface2CtrlData;
-    initInterface2CtrlData.guardToHandInMeter.setZero();
-    initInterface2CtrlData.guardToHandInMeter[1] = cfg->guardLength / 2;
-    initInterface2CtrlData.guardOriInRobotBase.setIdentity();
-    initInterface2CtrlData.desiredGuardOriInRobotBase.w() = cfg->defaultGuardOri[0];
-    initInterface2CtrlData.desiredGuardOriInRobotBase.x() = cfg->defaultGuardOri[1];
-    initInterface2CtrlData.desiredGuardOriInRobotBase.y() = cfg->defaultGuardOri[2];
-    initInterface2CtrlData.desiredGuardOriInRobotBase.z() = cfg->defaultGuardOri[3];
-    initInterface2CtrlData.guardRotationStiffness << cfg->defaultRotationStiffness[0], cfg->defaultRotationStiffness[1], cfg->defaultRotationStiffness[2];
-    initInterface2CtrlData.guardObsAvoidVel.setZero();
-    interface2CtrlData.reinitAllBuffers(initInterface2CtrlData);
-
-    // initial filter
-    left_desiredTorques_filtered.resize(left_torque_targets.size());
-    left_desiredTorques_filtered.setZero();
-    right_desiredTorques_filtered.resize(right_torque_targets.size());
-    right_desiredTorques_filtered.setZero();
-
-
-    left_currentTCPLinearVelocity_filtered.setZero();
-    right_currentTCPLinearVelocity_filtered.setZero();
-
-    left_currentTCPAngularVelocity_filtered.setZero();
-    right_currentTCPAngularVelocity_filtered.setZero();
-
-    left_tcpDesiredTorque_filtered.setZero();
-    right_tcpDesiredTorque_filtered.setZero();
-
-
-    smooth_startup = 0;
-
-    filterTimeConstant = cfg->filterTimeConstant;
-    posiKp = cfg->posiKp;
-    v_max = cfg->v_max;
-    Damping = cfg->posiDamping;
-    torqueLimit = cfg->torqueLimit;
-    null_torqueLimit = cfg->NullTorqueLimit;
-    oriKp = cfg->oriKp;
-    oriDamping  = cfg->oriDamping;
-
-    // nullspace control
-    left_qnullspace.resize(cfg->leftarm_qnullspaceVec.size());
-    right_qnullspace.resize(cfg->rightarm_qnullspaceVec.size());
-    for (size_t i = 0; i < cfg->leftarm_qnullspaceVec.size(); ++i)
-    {
-        left_qnullspace(i) = cfg->leftarm_qnullspaceVec[i];
-        right_qnullspace(i) = cfg->rightarm_qnullspaceVec[i];
-    }
-    nullspaceKp = cfg->nullspaceKp;
-    nullspaceDamping = cfg->nullspaceDamping;
-
-
-    //set GMM parameters ...
-    if (cfg->gmmParamsFile == "")
-    {
-        ARMARX_ERROR << "gmm params file cannot be empty string ...";
-    }
-    gmmMotionGenerator.reset(new JointCarryGMMMotionGen(cfg->gmmParamsFile));
-
-    ARMARX_INFO << "Initialization done";
-}
-
-void DSJointCarryController::controllerRun()
-{
-    if (!controllerSensorData.updateReadBuffer())
-    {
-        return;
-    }
-
-    // receive the measurements
-    Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
-    Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
-
-    Eigen::Vector3f left_currentTCPPositionInMeter;
-    Eigen::Vector3f right_currentTCPPositionInMeter;
-    left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3), left_currentTCPPose(2, 3);
-    right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3), right_currentTCPPose(2, 3);
-    left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
-    right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
-
-    interface2CtrlData.updateReadBuffer();
-    Eigen::Quaternionf  guard_currentOrientation = interface2CtrlData.getReadBuffer().guardOriInRobotBase;
-    Eigen::Quaternionf  guard_desiredOrientation = interface2CtrlData.getReadBuffer().desiredGuardOriInRobotBase;
-    Eigen::Vector3f guardToHandInMeter = interface2CtrlData.getReadBuffer().guardToHandInMeter;
-    Eigen::Vector3f guardRotationStiffness = interface2CtrlData.getReadBuffer().guardRotationStiffness;
-    Eigen::Vector3f guardModulatedVel = interface2CtrlData.getReadBuffer().guardObsAvoidVel;
-
-    Eigen::Vector3f left_to_right = right_currentTCPPositionInMeter - left_currentTCPPositionInMeter;
-    left_to_right.normalize();
-    // calculate the desired position velocity of the guard
-    Eigen::Vector3f guard_currentPosition;
-    guard_currentPosition = (left_currentTCPPositionInMeter + right_currentTCPPositionInMeter) / 2 + guardToHandInMeter;
-    gmmMotionGenerator->updateDesiredVelocity(guard_currentPosition, cfg->positionErrorTolerance);
-    Eigen::Vector3f guardDesiredLinearVelocity = gmmMotionGenerator->guard_desiredVelocity;
-
-
-    ctrl2InterfaceData.getWriteBuffer().guardZVel = guardDesiredLinearVelocity[2];
-    ctrl2InterfaceData.commitWrite();
-
-    guardDesiredLinearVelocity[2] = guardDesiredLinearVelocity[2] + guardModulatedVel[2];
-
-    // calculate the desired rotation velocity of the guard
-    if (guard_desiredOrientation.coeffs().dot(guard_currentOrientation.coeffs()) < 0.0)
-    {
-        guard_currentOrientation.coeffs() << - guard_currentOrientation.coeffs();
-    }
-    Eigen::Quaternionf errorOri = guard_currentOrientation * guard_desiredOrientation.inverse();
-    Eigen::AngleAxisf err_axang(errorOri);
-    Eigen::Vector3f guard_angular_error = err_axang.axis() * err_axang.angle();
-    Eigen::Vector3f guard_desired_angular_vel = -1 * guardRotationStiffness.cwiseProduct(guard_angular_error);
-
-    // calculate the hand desired linear velocity
-    Eigen::Vector3f guard_to_left = left_currentTCPPositionInMeter - guard_currentPosition;
-    Eigen::Vector3f leftOriGenLinearVelocity = guard_desired_angular_vel.cross(guard_to_left);
-    leftOriGenLinearVelocity -= leftOriGenLinearVelocity.dot(left_to_right) * left_to_right;
-    Eigen::Vector3f leftDesiredLinearVelocity = leftOriGenLinearVelocity + guardDesiredLinearVelocity;
-
-    if (leftDesiredLinearVelocity.norm() > cfg->handVelLimit)
-    {
-        leftDesiredLinearVelocity = cfg->handVelLimit * leftDesiredLinearVelocity / leftDesiredLinearVelocity.norm();
-    }
-
-    Eigen::Vector3f guard_to_right = right_currentTCPPositionInMeter - guard_currentPosition;
-    Eigen::Vector3f rightOriGenLinearVelocity = guard_desired_angular_vel.cross(guard_to_right);
-    rightOriGenLinearVelocity -= rightOriGenLinearVelocity.dot(left_to_right) * left_to_right;
-    Eigen::Vector3f rightDesiredLinearVelocity = rightOriGenLinearVelocity + guardDesiredLinearVelocity;
-
-    if (rightDesiredLinearVelocity.norm() > cfg->handVelLimit)
-    {
-        rightDesiredLinearVelocity = cfg->handVelLimit * rightDesiredLinearVelocity / rightDesiredLinearVelocity.norm();
-    }
-
-    // calculate the desired orientation of the hand
-    Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
-    Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
-    Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
-    Eigen::Matrix3f right_desiredRotMat = right_desiredQuaternion.normalized().toRotationMatrix();
-    Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
-    Eigen::Matrix3f right_orientationError = right_currentRotMat * right_desiredRotMat.inverse();
-    Eigen::Quaternionf left_diffQuaternion(left_orientationError);
-    Eigen::Quaternionf right_diffQuaternion(right_orientationError);
-    Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
-    Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
-    Eigen::Vector3f left_tcpDesiredAngularError = left_angleAxis.angle() * left_angleAxis.axis();
-    Eigen::Vector3f right_tcpDesiredAngularError = right_angleAxis.angle() * right_angleAxis.axis();
-
-    // writing to the buffer
-    LockGuardType guard {controlDataMutex};
-    getWriterControlStruct().leftDesiredLinearVelocity = leftDesiredLinearVelocity;
-    getWriterControlStruct().leftDesiredAngularError = left_tcpDesiredAngularError;
-    getWriterControlStruct().rightDesiredLinearVelocity = rightDesiredLinearVelocity;
-    getWriterControlStruct().rightDesiredAngularError = right_tcpDesiredAngularError;
-    writeControlStruct();
-
-    debugCtrlDataInfo.getWriteBuffer().leftDesiredLinearVelocity = leftDesiredLinearVelocity;
-    debugCtrlDataInfo.getWriteBuffer().rightDesiredLinearVelocity = rightDesiredLinearVelocity;
-    debugCtrlDataInfo.commitWrite();
-}
-
-
-void DSJointCarryController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
-    /* get sensor data */
-    double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-    Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
-    Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
-    Eigen::Vector3f left_forceInRoot = leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force);
-    Eigen::Vector3f right_forceInRoot = rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force);
-    left_forceInRoot(2) = left_forceInRoot(2); // - 4 + 8.5;
-    right_forceInRoot(2) = right_forceInRoot(2); // + 30 - 5.2;
-    Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
-    Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
-
-
-    Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-    Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-    Eigen::VectorXf left_qpos;
-    Eigen::VectorXf left_qvel;
-
-    Eigen::VectorXf right_qpos;
-    Eigen::VectorXf right_qvel;
-
-    left_qpos.resize(left_positionSensors.size());
-    left_qvel.resize(left_velocitySensors.size());
-
-    right_qpos.resize(right_positionSensors.size());
-    right_qvel.resize(right_velocitySensors.size());
-
-    int jointDim = left_positionSensors.size();
-
-    for (size_t i = 0; i < left_velocitySensors.size(); ++i)
-    {
-        left_qpos(i) = left_positionSensors[i]->position;
-        left_qvel(i) = left_velocitySensors[i]->velocity;
-
-        right_qpos(i) = right_positionSensors[i]->position;
-        right_qvel(i) = right_velocitySensors[i]->velocity;
-    }
-
-    Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
-    Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
-
-    Eigen::Vector3f left_currentTCPLinearVelocity;
-    Eigen::Vector3f right_currentTCPLinearVelocity;
-    Eigen::Vector3f left_currentTCPAngularVelocity;
-    Eigen::Vector3f right_currentTCPAngularVelocity;
-    left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0),  0.001 * left_tcptwist(1), 0.001 * left_tcptwist(2);
-    right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0),  0.001 * right_tcptwist(1), 0.001 * right_tcptwist(2);
-    left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
-    right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
-    double filterFactor = deltaT / (filterTimeConstant + deltaT);
-    left_currentTCPLinearVelocity_filtered  = (1 - filterFactor) * left_currentTCPLinearVelocity_filtered  + filterFactor * left_currentTCPLinearVelocity;
-    right_currentTCPLinearVelocity_filtered = (1 - filterFactor) * right_currentTCPLinearVelocity_filtered + filterFactor * right_currentTCPLinearVelocity;
-    left_currentTCPAngularVelocity_filtered = (1 - filterFactor) * left_currentTCPAngularVelocity_filtered + left_currentTCPAngularVelocity;
-    right_currentTCPAngularVelocity_filtered = (1 - filterFactor) * right_currentTCPAngularVelocity_filtered + right_currentTCPAngularVelocity;
-    left_jointVelocity_filtered  = (1 - filterFactor) * left_jointVelocity_filtered  + filterFactor * left_qvel;
-    right_jointVelocity_filtered  = (1 - filterFactor) * right_jointVelocity_filtered  + filterFactor * right_qvel;
-
-    controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
-    controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
-    controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
-    controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
-    controllerSensorData.getWriteBuffer().currentTime += deltaT;
-    controllerSensorData.commitWrite();
-
-
-    Eigen::Vector3f leftDesiredLinearVelocity = rtGetControlStruct().leftDesiredLinearVelocity;
-    Eigen::Vector3f rightDesiredLinearVelocity = rtGetControlStruct().rightDesiredLinearVelocity;
-    Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().leftDesiredAngularError;
-    Eigen::Vector3f right_tcpDesiredAngularError = rtGetControlStruct().rightDesiredAngularError;
-
-    // computing the task-specific forces
-    Eigen::Vector3f left_DS_force = -(left_currentTCPLinearVelocity_filtered - leftDesiredLinearVelocity);
-    Eigen::Vector3f right_DS_force = -(right_currentTCPLinearVelocity_filtered - rightDesiredLinearVelocity);
-    for (int i = 0; i < 3; ++i)
-    {
-        left_DS_force(i) = left_DS_force(i) * Damping[i];
-        right_DS_force(i) = right_DS_force(i) * Damping[i];
-        left_DS_force(i)  = deadzone(left_DS_force(i), 0.1, 100);
-        right_DS_force(i)  = deadzone(right_DS_force(i), 0.1, 100);
-    }
-
-    Eigen::Vector3f left_tcpDesiredTorque = - oriKp * left_tcpDesiredAngularError - oriDamping * left_currentTCPAngularVelocity_filtered;
-    Eigen::Vector3f right_tcpDesiredTorque = - oriKp * right_tcpDesiredAngularError - oriDamping * right_currentTCPAngularVelocity_filtered;
-
-    left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered  + filterFactor * left_tcpDesiredTorque;
-    right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered  + filterFactor * right_tcpDesiredTorque;
-
-    Eigen::Vector6f left_tcpDesiredWrench;
-    Eigen::Vector6f right_tcpDesiredWrench;
-
-    // times 0.001 because Jacobi matrix is in mm and radian.
-    left_tcpDesiredWrench << 0.001 * left_DS_force, left_tcpDesiredTorque_filtered;
-    right_tcpDesiredWrench << 0.001 * right_DS_force, right_tcpDesiredTorque_filtered;
-
-    //    left_tcpDesiredWrench(2) += 0.001 * cfg->guardGravity / 4;
-    //    right_tcpDesiredWrench(2) += 0.001 * cfg->guardGravity / 4;
-
-
-    // calculate the null-spcae torque
-    Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
-    float lambda = 0.2;
-    Eigen::MatrixXf left_jtpinv = left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
-    Eigen::MatrixXf right_jtpinv = right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
-    Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
-    Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
-
-    for (int i = 0; i < left_nullqerror.size(); ++i)
-    {
-        if (fabs(left_nullqerror(i)) < 0.09)
-        {
-            left_nullqerror(i) = 0;
-        }
-
-        if (fabs(right_nullqerror(i)) < 0.09)
-        {
-            right_nullqerror(i) = 0;
-        }
-    }
-
-    Eigen::VectorXf left_nullspaceTorque = - nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
-    Eigen::VectorXf right_nullspaceTorque = - nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
-    Eigen::VectorXf left_projectedNullTorque = (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
-    Eigen::VectorXf right_projectedNullTorque = (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
-    float left_nulltorque_norm = left_projectedNullTorque.norm();
-    float right_nulltorque_norm = right_projectedNullTorque.norm();
-    if (left_nulltorque_norm > null_torqueLimit)
-    {
-        left_projectedNullTorque = (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
-    }
-    if (right_nulltorque_norm > null_torqueLimit)
-    {
-        right_projectedNullTorque = (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
-    }
-
-    Eigen::VectorXf left_jointDesiredTorques = left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
-    Eigen::VectorXf right_jointDesiredTorques = right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
-    for (size_t i = 0; i < left_torque_targets.size(); ++i)
-    {
-        float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
-        desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
-        left_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
-
-        std::string names = left_jointNames[i] + "_desiredTorques";
-        debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
-        names = names + "_filtered";
-        debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
-
-        if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
-        {
-            left_torque_targets.at(i)->torque = 0;
-        }
-        else
-        {
-            left_torque_targets.at(i)->torque = left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
-        }
-    }
-
-    for (size_t i = 0; i < right_torque_targets.size(); ++i)
-    {
-        float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
-        desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
-        right_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
-
-        std::string names = right_jointNames[i] + "_desiredTorques";
-        debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
-        names = names + "_filtered";
-        debugDataInfo.getWriteBuffer().desired_torques[names] = right_desiredTorques_filtered(i);
-
-        if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
-        {
-            right_torque_targets.at(i)->torque = 0;
-        }
-        else
-        {
-            right_torque_targets.at(i)->torque = right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
-        }
-    }
-
-    smooth_startup = smooth_startup + 0.001 * (1.1  - smooth_startup);
-    smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
-    smooth_startup = (smooth_startup < 0) ?  0 : smooth_startup;
-
-    debugDataInfo.commitWrite();
-}
-
-void DSJointCarryController::setGuardOrientation(const Ice::FloatSeq& guardOrientationInRobotBase, const Ice::Current&)
-{
-    Eigen::Quaternionf guardOri;
-    guardOri.w() = guardOrientationInRobotBase[0];
-    guardOri.x() = guardOrientationInRobotBase[1];
-    guardOri.y() = guardOrientationInRobotBase[2];
-    guardOri.z() = guardOrientationInRobotBase[3];
-
-    LockGuardType guard {interface2CtrlDataMutex};
-    interface2CtrlData.getWriteBuffer().guardOriInRobotBase = guardOri;
-    interface2CtrlData.commitWrite();
-}
-
-void DSJointCarryController::setGuardInHandPosition(const Ice::FloatSeq& guardPositionToHandInMeter, const Ice::Current&)
-{
-    Eigen::Vector3f guardPosi;
-    guardPosi << guardPositionToHandInMeter[0], guardPositionToHandInMeter[1], guardPositionToHandInMeter[2];
-
-    LockGuardType guard {interface2CtrlDataMutex};
-    interface2CtrlData.getWriteBuffer().guardToHandInMeter = guardPosi;
-    interface2CtrlData.commitWrite();
-}
-
-void DSJointCarryController::setDesiredGuardOri(const Ice::FloatSeq& desiredOrientationInRobotBase, const Ice::Current&)
-{
-    Eigen::Quaternionf guardOri;
-    guardOri.w() = desiredOrientationInRobotBase[0];
-    guardOri.x() = desiredOrientationInRobotBase[1];
-    guardOri.y() = desiredOrientationInRobotBase[2];
-    guardOri.z() = desiredOrientationInRobotBase[3];
-
-    LockGuardType guard {interface2CtrlDataMutex};
-    interface2CtrlData.getWriteBuffer().desiredGuardOriInRobotBase = guardOri;
-    interface2CtrlData.commitWrite();
-}
-
-void DSJointCarryController::setRotationStiffness(const Ice::FloatSeq& rotationStiffness, const Ice::Current&)
-{
-    Eigen::Vector3f rotStiffness;
-    rotStiffness << rotationStiffness[0], rotStiffness[1], rotStiffness[2];
-
-    LockGuardType guard {interface2CtrlDataMutex};
-    interface2CtrlData.getWriteBuffer().guardRotationStiffness = rotStiffness;
-    interface2CtrlData.commitWrite();
-}
-
-void DSJointCarryController::setGuardObsAvoidVel(const Ice::FloatSeq& guardObsAvoidVel, const Ice::Current&)
-{
-    LockGuardType guard {interface2CtrlDataMutex};
-    interface2CtrlData.getWriteBuffer().guardObsAvoidVel(0) = guardObsAvoidVel[0];
-    interface2CtrlData.getWriteBuffer().guardObsAvoidVel(1) = guardObsAvoidVel[1];
-    interface2CtrlData.getWriteBuffer().guardObsAvoidVel(2) = guardObsAvoidVel[2];
-    interface2CtrlData.commitWrite();
-}
-
-float DSJointCarryController::getGMMVel(const Ice::Current&)
-{
-    float gmmZVel = ctrl2InterfaceData.getUpToDateReadBuffer().guardZVel;
-    return gmmZVel;
-}
-
-
-float DSJointCarryController::deadzone(float input, float disturbance, float threshold)
-{
-    if (input > 0)
-    {
-        input = input - disturbance;
-        input = (input < 0) ? 0 : input;
-        input = (input > threshold) ? threshold : input;
-    }
-    else if (input < 0)
-    {
-        input = input + disturbance;
-        input = (input > 0) ? 0 : input;
-        input = (input < -threshold) ? -threshold : input;
-    }
-
-
-    return input;
-}
-
-Eigen::Quaternionf DSJointCarryController::quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1)
-{
-    double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
-
-
-    Eigen::Quaternionf q1x = q1;
-    if (cosHalfTheta < 0)
-    {
-        q1x.w() = -q1.w();
-        q1x.x() = -q1.x();
-        q1x.y() = -q1.y();
-        q1x.z() = -q1.z();
-    }
-
-
-    if (fabs(cosHalfTheta) >= 1.0)
-    {
-        return q0;
-    }
-
-    double halfTheta = acos(cosHalfTheta);
-    double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
-
-
-    Eigen::Quaternionf result;
-    if (fabs(sinHalfTheta) < 0.001)
-    {
-        result.w() = (1 - t) * q0.w() + t * q1x.w();
-        result.x() = (1 - t) * q0.x() + t * q1x.x();
-        result.y() = (1 - t) * q0.y() + t * q1x.y();
-        result.z() = (1 - t) * q0.z() + t * q1x.z();
-
-    }
-    else
-    {
-        double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
-        double ratioB = sin(t * halfTheta) / sinHalfTheta;
-
-        result.w() = ratioA * q0.w() + ratioB * q1x.w();
-        result.x() = ratioA * q0.x() + ratioB * q1x.x();
-        result.y() = ratioA * q0.y() + ratioB * q1x.y();
-        result.z() = ratioA * q0.z() + ratioB * q1x.z();
-    }
-
-    return result;
-}
-
-void DSJointCarryController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-{
-
-    StringVariantBaseMap datafields;
-    auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
-    for (auto& pair : values)
-    {
-        datafields[pair.first] = new Variant(pair.second);
-    }
-
-
-    datafields["leftDesiredLinearVelocity_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[0]);
-    datafields["leftDesiredLinearVelocity_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[1]);
-    datafields["leftDesiredLinearVelocity_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[2]);
-    datafields["rightDesiredLinearVelocity_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[0]);
-    datafields["rightDesiredLinearVelocity_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[1]);
-    datafields["rightDesiredLinearVelocity_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[2]);
-    debugObs->setDebugChannel("DSJointCarry", datafields);
-
-}
-
-void DSJointCarryController::rtPreActivateController()
-{
-
-}
-
-void DSJointCarryController::rtPostDeactivateController()
-{
-
-}
-
diff --git a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h
deleted file mode 100644
index 8f360615d02c3ecb3d2a07192ee8013b8ee73abf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h
+++ /dev/null
@@ -1,339 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    DSController::ArmarXObjects::DSJointCarryController
- * @author     Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#ifndef _ARMARX_LIB_DSController_DSJointCarryController_H
-#define _ARMARX_LIB_DSController_DSJointCarryController_H
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Tools/Gravity.h>
-
-#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
-#include "GMRDynamics.h"
-#include <ArmarXCore/util/json/JSONObject.h>
-
-#include "MathLib.h"
-//#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
-//#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
-
-
-namespace armarx
-{
-
-    using JointCarryGMMPtr = std::shared_ptr<GMRDynamics>;
-
-    struct JointCarryGMRParameters
-    {
-        int K_gmm_;
-        int dim_;
-        std::vector<double> Priors_;
-        std::vector<double> Mu_;
-        std::vector<double> Sigma_;
-        std::vector<double> attractor_;
-        double dt_;
-    };
-
-
-    class JointCarryGMMMotionGen
-    {
-    public:
-        JointCarryGMMMotionGen() {}
-
-        JointCarryGMMMotionGen(const std::string& fileName)
-        {
-            getGMMParamsFromJsonFile(fileName);
-        }
-
-        JointCarryGMMPtr  guard_gmm;
-        JointCarryGMRParameters guard_gmmParas;
-        Eigen::Vector3f guard_target;
-        Eigen::Vector3f guard_desiredVelocity;
-
-        float scaling;
-        float v_max;
-
-        void getGMMParamsFromJsonFile(const std::string& fileName)
-        {
-            std::ifstream infile { fileName };
-            std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
-            JSONObjectPtr json = new JSONObject();
-            json->fromString(objDefs);
-            guard_gmmParas.K_gmm_ = json->getInt("K");
-            guard_gmmParas.dim_ = json->getInt("Dim");
-            json->getArray<double>("Priors", guard_gmmParas.Priors_);
-            json->getArray<double>("Mu", guard_gmmParas.Mu_);
-            json->getArray<double>("Attractor", guard_gmmParas.attractor_);
-            json->getArray<double>("Sigma", guard_gmmParas.Sigma_);
-            scaling = json->getDouble("Scaling");
-            v_max = json->getDouble("MaxVelocity");
-
-            guard_gmm.reset(new GMRDynamics(guard_gmmParas.K_gmm_, guard_gmmParas.dim_, guard_gmmParas.dt_, guard_gmmParas.Priors_, guard_gmmParas.Mu_, guard_gmmParas.Sigma_));
-            guard_gmm->initGMR(0, 2, 3, 5);
-            std::cout << "line 162." << std::endl;
-
-
-            for (int i = 0; i < 3; ++i)
-            {
-                guard_target(i) = guard_gmmParas.attractor_.at(i);
-            }
-
-            std::cout << "Finished GMM." << std::endl;
-
-        }
-
-
-        void updateDesiredVelocity(
-            const Eigen::Vector3f& positionInMeter,
-            float positionErrorToleranceInMeter)
-        {
-            MathLib::Vector position_error;
-            position_error.Resize(3);
-
-            MathLib::Vector desired_vel;
-            desired_vel.Resize(3);
-
-            Eigen::Vector3f positionError = positionInMeter - guard_target;
-            if (positionError.norm() < positionErrorToleranceInMeter)
-            {
-                positionError.setZero();
-            }
-
-            for (int i = 0; i < 3; ++i)
-            {
-                position_error(i) = positionError(i);
-            }
-
-            desired_vel = guard_gmm->getVelocity(position_error);
-
-            guard_desiredVelocity << desired_vel(0), desired_vel(1), desired_vel(2);
-
-            guard_desiredVelocity = scaling * guard_desiredVelocity;
-
-            float lenVec = guard_desiredVelocity.norm();
-
-            if (std::isnan(lenVec))
-            {
-                guard_desiredVelocity.setZero();
-            }
-
-            if (lenVec > v_max)
-            {
-                guard_desiredVelocity = (v_max / lenVec) * guard_desiredVelocity;
-            }
-        }
-
-
-
-    };
-
-    using JointCarryGMMMotionGenPtr = std::shared_ptr<JointCarryGMMMotionGen>;
-
-    class DSJointCarryControllerControlData
-    {
-    public:
-        Eigen::Vector3f leftDesiredLinearVelocity;
-        Eigen::Vector3f leftDesiredAngularError;
-        Eigen::Vector3f rightDesiredLinearVelocity;
-        Eigen::Vector3f rightDesiredAngularError;
-    };
-
-    /**
-        * @defgroup Library-DSJointCarryController DSJointCarryController
-        * @ingroup DSController
-        * A description of the library DSJointCarryController.
-        *
-        * @class DSJointCarryController
-        * @ingroup Library-DSJointCarryController
-        * @brief Brief description of class DSJointCarryController.
-        *
-        * Detailed description of class DSJointCarryController.
-        */
-
-    class DSJointCarryController : public NJointControllerWithTripleBuffer<DSJointCarryControllerControlData>, public DSJointCarryControllerInterface
-    {
-
-        // ManagedIceObject interface
-    protected:
-        void onInitNJointController();
-        void onDisconnectNJointController();
-
-
-        void controllerRun();
-
-
-
-        // NJointControllerInterface interface
-    public:
-        using ConfigPtrT = DSJointCarryControllerConfigPtr;
-
-        DSJointCarryController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-
-        std::string getClassName(const Ice::Current&) const
-        {
-            return "DSJointCarryController";
-        }
-
-        // NJointController interface
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // DSJointCarryControllerInterface interface
-    public:
-        void setGuardInHandPosition(const Ice::FloatSeq& guardPositionToHandInMeter,  const Ice::Current&);
-        void setGuardOrientation(const Ice::FloatSeq& guardOrientationInRobotBase, const Ice::Current&);
-        void setDesiredGuardOri(const Ice::FloatSeq& desiredOrientationInRobotBase, const Ice::Current&);
-        void setRotationStiffness(const Ice::FloatSeq& rotationStiffness, const Ice::Current&);
-        void setGuardObsAvoidVel(const Ice::FloatSeq& guardVel, const Ice::Current&);
-        float getGMMVel(const Ice::Current&);
-    private:
-        mutable MutexType interface2CtrlDataMutex;
-
-        float deadzone(float currentValue, float targetValue, float threshold);
-        Eigen::Quaternionf quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1);
-        JointCarryGMMMotionGenPtr gmmMotionGenerator;
-        struct DSJointCarryControllerSensorData
-        {
-            Eigen::Matrix4f left_tcpPose;
-            Eigen::Matrix4f right_tcpPose;
-            Eigen::Vector3f left_force;
-            Eigen::Vector3f right_force;
-            double currentTime;
-
-        };
-        TripleBuffer<DSJointCarryControllerSensorData> controllerSensorData;
-
-        struct DSCtrlDebugInfo
-        {
-            Eigen::Vector3f leftDesiredLinearVelocity;
-            Eigen::Vector3f rightDesiredLinearVelocity;
-        };
-        TripleBuffer<DSCtrlDebugInfo> debugCtrlDataInfo;
-
-        struct Interface2CtrlData
-        {
-            Eigen::Vector3f guardToHandInMeter;
-            Eigen::Quaternionf guardOriInRobotBase;
-            Eigen::Quaternionf desiredGuardOriInRobotBase;
-            Eigen::Vector3f guardRotationStiffness;
-            Eigen::Vector3f guardObsAvoidVel;
-        };
-        TripleBuffer<Interface2CtrlData> interface2CtrlData;
-
-        struct DSRTDebugInfo
-        {
-            StringFloatDictionary desired_torques;
-        };
-        TripleBuffer<DSRTDebugInfo> debugDataInfo;
-
-        struct Ctrl2InterfaceData
-        {
-            float guardZVel;
-        };
-        TripleBuffer<Ctrl2InterfaceData> ctrl2InterfaceData;
-
-        std::vector<const SensorValue1DoFActuatorTorque*> left_torqueSensors;
-        std::vector<const SensorValue1DoFGravityTorque*> left_gravityTorqueSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> left_velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> left_positionSensors;
-
-        std::vector<const SensorValue1DoFActuatorTorque*> right_torqueSensors;
-        std::vector<const SensorValue1DoFGravityTorque*> right_gravityTorqueSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> right_velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> right_positionSensors;
-
-        const SensorValueForceTorque* leftForceTorque;
-        const SensorValueForceTorque* rightForceTorque;
-
-        std::vector<ControlTarget1DoFActuatorTorque*> left_torque_targets;
-        std::vector<ControlTarget1DoFActuatorTorque*> right_torque_targets;
-
-        VirtualRobot::RobotNodePtr left_arm_tcp;
-        VirtualRobot::RobotNodePtr right_arm_tcp;
-
-        VirtualRobot::RobotNodePtr left_sensor_frame;
-        VirtualRobot::RobotNodePtr right_sensor_frame;
-
-        VirtualRobot::DifferentialIKPtr left_ik;
-        VirtualRobot::DifferentialIKPtr right_ik;
-
-        Eigen::Quaternionf left_desiredQuaternion;
-        Eigen::Quaternionf right_desiredQuaternion;
-
-        Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
-        Eigen::Vector3f left_currentTCPAngularVelocity_filtered;
-        Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
-        Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
-
-        Eigen::VectorXf left_jointVelocity_filtered;
-        Eigen::VectorXf right_jointVelocity_filtered;
-
-        Eigen::VectorXf left_desiredTorques_filtered;
-        Eigen::VectorXf right_desiredTorques_filtered;
-        Eigen::Vector3f left_tcpDesiredTorque_filtered;
-        Eigen::Vector3f right_tcpDesiredTorque_filtered;
-
-        float smooth_startup;
-
-        DSJointCarryControllerConfigPtr cfg;
-        float filterTimeConstant;
-
-        std::vector<std::string> left_jointNames;
-        std::vector<std::string> right_jointNames;
-
-        float posiKp;
-        float v_max;
-        std::vector<float> Damping;
-        float oriKp;
-        float oriDamping;
-        float torqueLimit;
-        float null_torqueLimit;
-        float nullspaceKp;
-        float nullspaceDamping;
-        Eigen::VectorXf left_qnullspace;
-        Eigen::VectorXf right_qnullspace;
-
-
-        Eigen::Quaternionf desired_guardOri;
-
-        float positionErrorTolerance;
-        bool controllerStopRequested = false;
-        bool controllerRunFinished = false;
-
-        // NJointController interface
-    protected:
-        void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        // NJointController interface
-    protected:
-        void rtPreActivateController();
-        void rtPostDeactivateController();
-
-    };
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
deleted file mode 100644
index f7e120cde12892908f3f8937accbef17c1a824e1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
+++ /dev/null
@@ -1,1012 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    DSController::ArmarXObjects::DSRTBimanualController
- * @author     Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "DSRTBimanualController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<DSRTBimanualController> registrationControllerDSRTBimanualController("DSRTBimanualController");
-
-    void DSRTBimanualController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        controllerStopRequested = false;
-        controllerRunFinished = false;
-        runTask("DSRTBimanualControllerTask", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
-            {
-                ARMARX_VERBOSE << deactivateSpam(1) << "control fct";
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-            controllerRunFinished = true;
-            ARMARX_INFO << "Control Fct finished";
-        });
-
-
-    }
-
-    void DSRTBimanualController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "disconnect";
-        controllerStopRequested = true;
-        while (!controllerRunFinished)
-        {
-            ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
-            usleep(10000);
-        }
-    }
-
-
-    DSRTBimanualController::DSRTBimanualController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        cfg = DSRTBimanualControllerConfigPtr::dynamicCast(config);
-        useSynchronizedRtRobot();
-
-        VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
-        VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
-
-        left_jointNames.clear();
-        right_jointNames.clear();
-
-        ARMARX_CHECK_EXPRESSION(left_ns) << "LeftArm";
-        ARMARX_CHECK_EXPRESSION(right_ns) << "RightArm";
-
-        // for left arm
-        for (size_t i = 0; i < left_ns->getSize(); ++i)
-        {
-            std::string jointName = left_ns->getNode(i)->getName();
-            left_jointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            ARMARX_CHECK_EXPRESSION(ct);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            ARMARX_CHECK_EXPRESSION(sv);
-            auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
-            ARMARX_CHECK_EXPRESSION(casted_ct);
-            left_torque_targets.push_back(casted_ct);
-
-            const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            if (!torqueSensor)
-            {
-                ARMARX_WARNING << "No Torque sensor available for " << jointName;
-            }
-            if (!gravityTorqueSensor)
-            {
-                ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
-            }
-
-            left_torqueSensors.push_back(torqueSensor);
-            left_gravityTorqueSensors.push_back(gravityTorqueSensor);
-            left_velocitySensors.push_back(velocitySensor);
-            left_positionSensors.push_back(positionSensor);
-        };
-
-        // for right arm
-        for (size_t i = 0; i < right_ns->getSize(); ++i)
-        {
-            std::string jointName = right_ns->getNode(i)->getName();
-            right_jointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            ARMARX_CHECK_EXPRESSION(ct);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            ARMARX_CHECK_EXPRESSION(sv);
-            auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
-            ARMARX_CHECK_EXPRESSION(casted_ct);
-            right_torque_targets.push_back(casted_ct);
-
-            const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            if (!torqueSensor)
-            {
-                ARMARX_WARNING << "No Torque sensor available for " << jointName;
-            }
-            if (!gravityTorqueSensor)
-            {
-                ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
-            }
-
-            right_torqueSensors.push_back(torqueSensor);
-            right_gravityTorqueSensors.push_back(gravityTorqueSensor);
-            right_velocitySensors.push_back(velocitySensor);
-            right_positionSensors.push_back(positionSensor);
-        };
-
-
-        const SensorValueBase* svlf = useSensorValue("FT L");
-        leftForceTorque = svlf->asA<SensorValueForceTorque>();
-        const SensorValueBase* svrf = useSensorValue("FT R");
-        rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
-        ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
-        ARMARX_INFO << "Initialized " << right_torque_targets.size() << " targets for the right arm";
-
-        left_arm_tcp =  left_ns->getTCP();
-        right_arm_tcp =  right_ns->getTCP();
-
-        left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL8_Wri2");
-        right_sensor_frame  = right_ns->getRobot()->getRobotNode("ArmR8_Wri2");
-
-        left_ik.reset(new VirtualRobot::DifferentialIK(left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        right_ik.reset(new VirtualRobot::DifferentialIK(right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-        // ?? not sure about this
-        DSRTBimanualControllerSensorData initSensorData;
-
-        initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
-        initSensorData.currentTime = 0;
-        controllerSensorData.reinitAllBuffers(initSensorData);
-
-        initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
-        initSensorData.currentTime = 0;
-        controllerSensorData.reinitAllBuffers(initSensorData);
-
-
-        left_oldPosition = left_arm_tcp->getPositionInRootFrame();
-        left_oldOrientation = left_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
-
-        right_oldPosition = right_arm_tcp->getPositionInRootFrame();
-        right_oldOrientation = right_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
-
-
-        std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
-        ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
-
-        std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
-        ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
-
-        left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
-        left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
-        left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
-        left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
-
-        right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
-        right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
-        right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
-        right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
-
-
-        // set initial joint velocities filter
-
-        left_jointVelocity_filtered.resize(left_torque_targets.size());
-        left_jointVelocity_filtered.setZero();
-        right_jointVelocity_filtered.resize(left_torque_targets.size());
-        right_jointVelocity_filtered.setZero();
-
-        // do we need to duplicate this?
-        DSRTBimanualControllerControlData initData;
-        for (size_t i = 0; i < 3; ++i)
-        {
-            initData.left_tcpDesiredLinearVelocity(i) = 0;
-            initData.right_tcpDesiredLinearVelocity(i) = 0;
-
-        }
-
-        for (size_t i = 0; i < 3; ++i)
-        {
-            initData.left_tcpDesiredAngularError(i) = 0;
-            initData.right_tcpDesiredAngularError(i) = 0;
-
-        }
-        reinitTripleBuffer(initData);
-
-
-        // initial filter
-        left_desiredTorques_filtered.resize(left_torque_targets.size());
-        left_desiredTorques_filtered.setZero();
-        right_desiredTorques_filtered.resize(right_torque_targets.size());
-        right_desiredTorques_filtered.setZero();
-
-
-        left_currentTCPLinearVelocity_filtered.setZero();
-        right_currentTCPLinearVelocity_filtered.setZero();
-
-        left_currentTCPAngularVelocity_filtered.setZero();
-        right_currentTCPAngularVelocity_filtered.setZero();
-
-        left_tcpDesiredTorque_filtered.setZero();
-        right_tcpDesiredTorque_filtered.setZero();
-
-
-        smooth_startup = 0;
-
-
-        filterTimeConstant = cfg->filterTimeConstant;
-        posiKp = cfg->posiKp;
-        v_max = cfg->v_max;
-        Damping = cfg->posiDamping;
-        Coupling_stiffness = cfg->couplingStiffness;
-        Coupling_force_limit = cfg->couplingForceLimit;
-        forward_gain = cfg->forwardGain;
-        torqueLimit = cfg->torqueLimit;
-        null_torqueLimit = cfg->NullTorqueLimit;
-        oriKp = cfg->oriKp;
-        oriDamping  = cfg->oriDamping;
-        contactForce = cfg->contactForce;
-
-        left_oriUp.w() =  cfg->left_oriUp[0];
-        left_oriUp.x() =  cfg->left_oriUp[1];
-        left_oriUp.y() =  cfg->left_oriUp[2];
-        left_oriUp.z() =  cfg->left_oriUp[3];
-
-        left_oriDown.w() = cfg->left_oriDown[0];
-        left_oriDown.x() = cfg->left_oriDown[1];
-        left_oriDown.y() = cfg->left_oriDown[2];
-        left_oriDown.z() = cfg->left_oriDown[3];
-
-        right_oriUp.w() = cfg->right_oriUp[0];
-        right_oriUp.x() = cfg->right_oriUp[1];
-        right_oriUp.y() = cfg->right_oriUp[2];
-        right_oriUp.z() = cfg->right_oriUp[3];
-
-        right_oriDown.w() = cfg->right_oriDown[0];
-        right_oriDown.x() = cfg->right_oriDown[1];
-        right_oriDown.y() = cfg->right_oriDown[2];
-        right_oriDown.z() = cfg->right_oriDown[3];
-
-
-        guardTargetZUp = cfg->guardTargetZUp;
-        guardTargetZDown = cfg->guardTargetZDown;
-        guardDesiredZ = guardTargetZUp;
-        guard_mounting_correction_z = 0;
-
-        guardXYHighFrequency = 0;
-        left_force_old.setZero();
-        right_force_old.setZero();
-
-        left_contactForceZ_correction = 0;
-        right_contactForceZ_correction = 0;
-
-
-        std::vector<float> leftarm_qnullspaceVec = cfg->leftarm_qnullspaceVec;
-        std::vector<float> rightarm_qnullspaceVec = cfg->rightarm_qnullspaceVec;
-
-        left_qnullspace.resize(leftarm_qnullspaceVec.size());
-        right_qnullspace.resize(rightarm_qnullspaceVec.size());
-
-        for (size_t i = 0; i < leftarm_qnullspaceVec.size(); ++i)
-        {
-            left_qnullspace(i) = leftarm_qnullspaceVec[i];
-            right_qnullspace(i) = rightarm_qnullspaceVec[i];
-        }
-
-        nullspaceKp = cfg->nullspaceKp;
-        nullspaceDamping = cfg->nullspaceDamping;
-
-
-        //set GMM parameters ...
-        if (cfg->gmmParamsFile == "")
-        {
-            ARMARX_ERROR << "gmm params file cannot be empty string ...";
-        }
-        gmmMotionGenerator.reset(new BimanualGMMMotionGen(cfg->gmmParamsFile));
-        positionErrorTolerance = cfg->positionErrorTolerance;
-        forceFilterCoeff = cfg->forceFilterCoeff;
-        for (size_t i = 0 ; i < 3; ++i)
-        {
-            leftForceOffset[i] = cfg->forceLeftOffset.at(i);
-            rightForceOffset[i] = cfg->forceRightOffset.at(i);
-        }
-        isDefaultTarget = false;
-        ARMARX_INFO << "Initialization done";
-    }
-
-    void DSRTBimanualController::controllerRun()
-    {
-        if (!controllerSensorData.updateReadBuffer())
-        {
-            return;
-        }
-
-
-        // receive the measurements
-        Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
-        Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
-
-        Eigen::Vector3f left_force = controllerSensorData.getReadBuffer().left_force;
-        Eigen::Vector3f right_force = controllerSensorData.getReadBuffer().right_force;
-        Eigen::Vector3f both_arm_force_ave = - 0.5 * (left_force + right_force); // negative sign for the direction
-
-
-        //    float left_force_z = left_force(2);
-        //    float right_force_z = right_force(2);
-
-        Eigen::Vector3f left_currentTCPPositionInMeter;
-        Eigen::Vector3f right_currentTCPPositionInMeter;
-
-        left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3), left_currentTCPPose(2, 3);
-        right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3), right_currentTCPPose(2, 3);
-
-        left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
-        right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
-
-
-        // updating the desired height for the guard based on the interaction forces
-        float both_arm_height_z_ave =  0.5 * (left_currentTCPPositionInMeter(2) + right_currentTCPPositionInMeter(2));
-
-
-        float adaptive_ratio = 1;
-        float force_error_z = 0;
-        float guard_mounting_correction_x = 0;
-        float guard_mounting_correction_y = 0;
-
-
-        if (cfg->guardDesiredDirection)
-        {
-            adaptive_ratio = 1;
-            force_error_z = both_arm_force_ave(2) - adaptive_ratio  * cfg->liftingForce;
-
-            // meausures the high-frequency components of the interaction forces
-            float diff_norm = (left_force - left_force_old).squaredNorm() + (right_force - right_force_old).squaredNorm();
-            // diff_norm = deadzone(diff_norm,0.1,2);
-            guardXYHighFrequency = cfg->highPassFilterFactor * guardXYHighFrequency + 0.1 * diff_norm;
-
-            guardXYHighFrequency = (guardXYHighFrequency > 10) ? 10 : guardXYHighFrequency;
-
-            left_force_old = left_force;
-            right_force_old = right_force;
-
-            if (fabs(both_arm_height_z_ave - guardTargetZUp) < cfg->mountingDistanceTolerance)
-            {
-                guard_mounting_correction_z = (1 - cfg->mountingCorrectionFilterFactor) * guard_mounting_correction_z + cfg->mountingCorrectionFilterFactor * (- 0.1 * (guardXYHighFrequency - 8));
-                guard_mounting_correction_z = deadzone(guard_mounting_correction_z, 0, 0.1);
-
-
-                guard_mounting_correction_x = guard_mounting_correction_x - cfg->forceErrorZGain * deadzone(both_arm_force_ave(0), 1, 3);
-                guard_mounting_correction_y = guard_mounting_correction_y - cfg->forceErrorZGain * deadzone(both_arm_force_ave(1), 1, 3);
-
-                guard_mounting_correction_x = deadzone(guard_mounting_correction_x, 0, 0.1);
-                guard_mounting_correction_y = deadzone(guard_mounting_correction_y, 0, 0.1);
-
-
-            }
-
-        }
-        else
-        {
-            adaptive_ratio = (0.5 * (both_arm_height_z_ave - guardTargetZDown) / (guardTargetZUp - guardTargetZDown) + 0.5);
-            force_error_z = both_arm_force_ave(2) - adaptive_ratio  * cfg->loweringForce;
-
-        }
-
-
-
-        force_error_z = deadzone(force_error_z, cfg->forceErrorZDisturbance, cfg->forceErrorZThreshold);
-        guardDesiredZ = guardDesiredZ - cfg->forceErrorZGain * (force_error_z);
-
-        guardDesiredZ = (guardDesiredZ > guardTargetZUp) ?  guardTargetZUp : guardDesiredZ;
-        guardDesiredZ = (guardDesiredZ < guardTargetZDown) ?  guardTargetZDown : guardDesiredZ;
-
-        if (isDefaultTarget)
-        {
-            guardDesiredZ = guardTargetZDown;
-        }
-
-        if (!cfg->guardDesiredDirection && guardDesiredZ < 1.5)
-        {
-            guard_mounting_correction_y = -0.1;
-        }
-
-        // update the desired velocity
-        gmmMotionGenerator->updateDesiredVelocity(
-            left_currentTCPPositionInMeter,
-            right_currentTCPPositionInMeter,
-            positionErrorTolerance,
-            guardDesiredZ,
-            guard_mounting_correction_x,
-            guard_mounting_correction_y,
-            guard_mounting_correction_z);
-
-        //    ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->left_DS_DesiredVelocity;
-        //    ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->right_DS_DesiredVelocity;
-
-
-        Eigen::Vector3f left_tcpDesiredAngularError;
-        Eigen::Vector3f right_tcpDesiredAngularError;
-
-        left_tcpDesiredAngularError << 0, 0, 0;
-        right_tcpDesiredAngularError << 0, 0, 0;
-
-
-
-        Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
-        Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
-
-
-        float lqratio = (left_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
-        float rqratio = (right_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
-
-        lqratio = (lqratio > 1) ?  1 : lqratio;
-        lqratio = (lqratio < 0) ?  0 : lqratio;
-        rqratio = (rqratio > 1) ?  1 : rqratio;
-        rqratio = (rqratio < 0) ?  0 : rqratio;
-
-
-        Eigen::Quaternionf left_desiredQuaternion = quatSlerp(lqratio, left_oriDown, left_oriUp);
-        Eigen::Quaternionf right_desiredQuaternion = quatSlerp(rqratio, right_oriDown, right_oriUp);
-
-
-        Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
-        Eigen::Matrix3f right_desiredRotMat = right_desiredQuaternion.normalized().toRotationMatrix();
-
-        Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
-        Eigen::Matrix3f right_orientationError = right_currentRotMat * right_desiredRotMat.inverse();
-
-        Eigen::Quaternionf left_diffQuaternion(left_orientationError);
-        Eigen::Quaternionf right_diffQuaternion(right_orientationError);
-
-        Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
-        Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
-
-        left_tcpDesiredAngularError = left_angleAxis.angle() * left_angleAxis.axis();
-        right_tcpDesiredAngularError = right_angleAxis.angle() * right_angleAxis.axis();
-
-
-        // writing to the buffer
-        getWriterControlStruct().left_tcpDesiredLinearVelocity = gmmMotionGenerator->left_DS_DesiredVelocity;
-        getWriterControlStruct().right_tcpDesiredLinearVelocity = gmmMotionGenerator->right_DS_DesiredVelocity;
-        getWriterControlStruct().left_right_distanceInMeter = gmmMotionGenerator->left_right_position_errorInMeter;
-
-        getWriterControlStruct().left_tcpDesiredAngularError = left_tcpDesiredAngularError;
-        getWriterControlStruct().right_tcpDesiredAngularError = right_tcpDesiredAngularError;
-
-        writeControlStruct();
-        debugCtrlDataInfo.getWriteBuffer().desredZ = guardDesiredZ;
-        debugCtrlDataInfo.getWriteBuffer().force_error_z = force_error_z;
-        debugCtrlDataInfo.getWriteBuffer().guardXYHighFrequency = guardXYHighFrequency;
-        debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_x = guard_mounting_correction_x;
-        debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_y = guard_mounting_correction_y;
-        debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_z = guard_mounting_correction_z;
-        debugCtrlDataInfo.commitWrite();
-
-    }
-
-
-    void DSRTBimanualController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        // reading the control objectives from the other threads (desired velocities)
-        Eigen::Vector3f left_tcpDesiredLinearVelocity = rtGetControlStruct().left_tcpDesiredLinearVelocity;
-        Eigen::Vector3f right_tcpDesiredLinearVelocity = rtGetControlStruct().right_tcpDesiredLinearVelocity;
-        Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().left_tcpDesiredAngularError;
-        Eigen::Vector3f right_tcpDesiredAngularError = rtGetControlStruct().right_tcpDesiredAngularError;
-        Eigen::Vector3f left_right_distanceInMeter = rtGetControlStruct().left_right_distanceInMeter;
-
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-
-        // measure the sesor data for the other threads
-
-        Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
-        Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
-
-        //    ARMARX_IMPORTANT << "left force offset: " << leftForceOffset;
-        //    ARMARX_IMPORTANT << "right force offset: " << rightForceOffset;
-
-        Eigen::Vector3f left_forceInRoot = leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force - leftForceOffset);
-        Eigen::Vector3f right_forceInRoot = rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force - rightForceOffset);
-        //    Eigen::Vector3f left_torqueInRoot = leftSensorFrame.block<3, 3>(0, 0) * leftForceTorque->torque;
-        //    Eigen::Vector3f right_torqueInRoot = rightSensorFrame.block<3, 3>(0, 0) * rightForceTorque->torque;
-        left_forceInRoot(2) = left_forceInRoot(2) + cfg->leftForceOffsetZ; // - 4 + 8.5;
-        right_forceInRoot(2) = right_forceInRoot(2) + cfg->rightForceOffsetZ; // + 30 - 5.2;
-
-
-        Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
-        Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
-
-
-        Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-        Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf left_qpos;
-        Eigen::VectorXf left_qvel;
-
-        Eigen::VectorXf right_qpos;
-        Eigen::VectorXf right_qvel;
-
-        left_qpos.resize(left_positionSensors.size());
-        left_qvel.resize(left_velocitySensors.size());
-
-        right_qpos.resize(right_positionSensors.size());
-        right_qvel.resize(right_velocitySensors.size());
-
-        int jointDim = left_positionSensors.size();
-
-        for (size_t i = 0; i < left_velocitySensors.size(); ++i)
-        {
-            left_qpos(i) = left_positionSensors[i]->position;
-            left_qvel(i) = left_velocitySensors[i]->velocity;
-
-            right_qpos(i) = right_positionSensors[i]->position;
-            right_qvel(i) = right_velocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
-        Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
-
-        Eigen::Vector3f left_currentTCPLinearVelocity;
-        Eigen::Vector3f right_currentTCPLinearVelocity;
-        Eigen::Vector3f left_currentTCPAngularVelocity;
-        Eigen::Vector3f right_currentTCPAngularVelocity;
-        left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0),  0.001 * left_tcptwist(1), 0.001 * left_tcptwist(2);
-        right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0),  0.001 * right_tcptwist(1), 0.001 * right_tcptwist(2);
-        left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
-        right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
-        double filterFactor = deltaT / (filterTimeConstant + deltaT);
-        left_currentTCPLinearVelocity_filtered  = (1 - filterFactor) * left_currentTCPLinearVelocity_filtered  + filterFactor * left_currentTCPLinearVelocity;
-        right_currentTCPLinearVelocity_filtered = (1 - filterFactor) * right_currentTCPLinearVelocity_filtered + filterFactor * right_currentTCPLinearVelocity;
-        left_currentTCPAngularVelocity_filtered = (1 - filterFactor) * left_currentTCPAngularVelocity_filtered + left_currentTCPAngularVelocity;
-        right_currentTCPAngularVelocity_filtered = (1 - filterFactor) * right_currentTCPAngularVelocity_filtered + right_currentTCPAngularVelocity;
-        left_jointVelocity_filtered  = (1 - filterFactor) * left_jointVelocity_filtered  + filterFactor * left_qvel;
-        right_jointVelocity_filtered  = (1 - filterFactor) * right_jointVelocity_filtered  + filterFactor * right_qvel;
-
-        // writing into the bufffer for other threads
-        controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
-        controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
-        controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
-        controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
-        controllerSensorData.getWriteBuffer().currentTime += deltaT;
-        controllerSensorData.commitWrite();
-
-
-
-
-        // inverse dynamics:
-
-
-        // reading the tcp orientation
-
-
-
-
-        // computing the task-specific forces
-        Eigen::Vector3f left_DS_force = -(1.2 * left_currentTCPLinearVelocity_filtered - forward_gain * left_tcpDesiredLinearVelocity);
-        Eigen::Vector3f right_DS_force = -(1.2 * right_currentTCPLinearVelocity_filtered - forward_gain * right_tcpDesiredLinearVelocity);
-        for (int i = 0; i < 3; ++i)
-        {
-            left_DS_force(i) = left_DS_force(i) * Damping[i];
-            right_DS_force(i) = right_DS_force(i) * Damping[i];
-
-            left_DS_force(i)  = deadzone(left_DS_force(i), 0.1, 100);
-            right_DS_force(i)  = deadzone(right_DS_force(i), 0.1, 100);
-
-        }
-
-        // computing coupling forces
-        Eigen::Vector3f coupling_force = - Coupling_stiffness * left_right_distanceInMeter;
-        float coupling_force_norm = coupling_force.norm();
-
-        if (coupling_force_norm > Coupling_force_limit)
-        {
-            coupling_force = (Coupling_force_limit / coupling_force_norm) * coupling_force;
-        }
-
-        coupling_force(0)  = deadzone(coupling_force(0), 0.1, 100);
-        coupling_force(1)  = deadzone(coupling_force(1), 0.1, 100);
-        coupling_force(2)  = deadzone(coupling_force(2), 0.1, 100);
-
-
-
-
-        double v_both = left_currentTCPLinearVelocity_filtered.norm() + right_currentTCPLinearVelocity_filtered.norm();
-        float force_contact_switch = 0;
-        float left_height = left_currentTCPPose(2, 3) * 0.001;
-        float right_height = right_currentTCPPose(2, 3) * 0.001;
-
-        float disTolerance = cfg->contactDistanceTolerance;
-        bool isUp =  fabs(left_height - guardTargetZUp) <  disTolerance && fabs(right_height - guardTargetZUp) < disTolerance;
-        if (v_both < disTolerance && isUp)
-        {
-            force_contact_switch = 1;
-        }
-        else if (v_both < 0.05 &&  isUp)
-        {
-            force_contact_switch = (0.05 - v_both) / (0.05 - disTolerance);
-        }
-        else if (v_both >= 0.05)
-        {
-            force_contact_switch = 0;
-        }
-
-        // computing for contact forces
-        float left_force_error = force_contact_switch * (-left_forceInRoot(2) -  contactForce);
-        float right_force_error = force_contact_switch * (-right_forceInRoot(2) -  contactForce);
-
-        //    float left_force_error_limited = left_force_error;
-        //    float right_force_error_limited = right_force_error;
-
-        //    left_force_error_limited = (left_force_error_limited >  2) ? 2 : left_force_error_limited;
-        //    left_force_error_limited = (left_force_error_limited < -2) ? -2 : left_force_error_limited;
-
-        //    right_force_error_limited = (right_force_error_limited >  2) ? 2 : right_force_error_limited;
-        //    right_force_error_limited = (right_force_error_limited < -2) ? -2 : right_force_error_limited;
-
-
-        left_force_error  = deadzone(left_force_error, 0.5, 2);
-        right_force_error  = deadzone(right_force_error, 0.5, 2);
-
-        left_contactForceZ_correction = left_contactForceZ_correction -  forceFilterCoeff * left_force_error;
-        right_contactForceZ_correction = right_contactForceZ_correction -  forceFilterCoeff * right_force_error;
-
-        left_contactForceZ_correction = (left_contactForceZ_correction > 30) ? 30 : left_contactForceZ_correction;
-        right_contactForceZ_correction = (right_contactForceZ_correction > 30) ? 30 : right_contactForceZ_correction;
-
-        left_contactForceZ_correction = (left_contactForceZ_correction < -30) ? -contactForce : left_contactForceZ_correction;
-        right_contactForceZ_correction = (right_contactForceZ_correction < -30) ? -contactForce : right_contactForceZ_correction;
-
-        Eigen::Vector3f left_tcpDesiredForce = left_DS_force + coupling_force;
-        Eigen::Vector3f right_tcpDesiredForce = right_DS_force - coupling_force;
-
-        left_tcpDesiredForce(2) += force_contact_switch * (contactForce + left_contactForceZ_correction);
-        right_tcpDesiredForce(2) += force_contact_switch * (contactForce + right_contactForceZ_correction);
-
-
-
-        Eigen::Vector3f left_tcpDesiredTorque = - oriKp * left_tcpDesiredAngularError - oriDamping * left_currentTCPAngularVelocity_filtered;
-        Eigen::Vector3f right_tcpDesiredTorque = - oriKp * right_tcpDesiredAngularError - oriDamping * right_currentTCPAngularVelocity_filtered;
-
-        //    for (int i = 0; i < left_tcpDesiredTorque.size(); ++i)
-        //    {
-        //        left_tcpDesiredTorque(i) = deadzone(left_tcpDesiredTorque(i), )
-
-        //    }
-
-
-        left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered  + filterFactor * left_tcpDesiredTorque;
-        right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered  + filterFactor * right_tcpDesiredTorque;
-
-
-        Eigen::Vector6f left_tcpDesiredWrench;
-        Eigen::Vector6f right_tcpDesiredWrench;
-
-        left_tcpDesiredWrench << 0.001 * left_tcpDesiredForce, left_tcpDesiredTorque_filtered;
-        right_tcpDesiredWrench << 0.001 * right_tcpDesiredForce, right_tcpDesiredTorque_filtered;
-
-
-        // calculate the null-spcae torque
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
-
-        float lambda = 0.2;
-        Eigen::MatrixXf left_jtpinv = left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
-        Eigen::MatrixXf right_jtpinv = right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
-
-
-        Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
-        Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
-
-        for (int i = 0; i < left_nullqerror.size(); ++i)
-        {
-            if (fabs(left_nullqerror(i)) < 0.09)
-            {
-                left_nullqerror(i) = 0;
-            }
-
-            if (fabs(right_nullqerror(i)) < 0.09)
-            {
-                right_nullqerror(i) = 0;
-            }
-        }
-
-
-        Eigen::VectorXf left_nullspaceTorque = - nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
-        Eigen::VectorXf right_nullspaceTorque = - nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
-
-        Eigen::VectorXf left_projectedNullTorque = (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
-        Eigen::VectorXf right_projectedNullTorque = (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
-
-        float left_nulltorque_norm = left_projectedNullTorque.norm();
-        float right_nulltorque_norm = right_projectedNullTorque.norm();
-
-        if (left_nulltorque_norm > null_torqueLimit)
-        {
-            left_projectedNullTorque = (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
-        }
-
-        if (right_nulltorque_norm > null_torqueLimit)
-        {
-            right_projectedNullTorque = (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
-        }
-
-        Eigen::VectorXf left_jointDesiredTorques = left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
-        Eigen::VectorXf right_jointDesiredTorques = right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
-
-
-        right_desiredTorques_filtered = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered + cfg->TorqueFilterConstant * right_jointDesiredTorques;
-
-
-        for (size_t i = 0; i < left_torque_targets.size(); ++i)
-        {
-            float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
-            desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
-            left_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
-
-            std::string names = left_jointNames[i] + "_desiredTorques";
-            debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
-            names = names + "_filtered";
-            debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
-
-            if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
-            {
-                left_torque_targets.at(i)->torque = 0;
-            }
-            else
-            {
-                left_torque_targets.at(i)->torque = left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
-            }
-        }
-
-        for (size_t i = 0; i < right_torque_targets.size(); ++i)
-        {
-            float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
-            desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
-            right_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
-
-            std::string names = right_jointNames[i] + "_desiredTorques";
-            debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
-            names = names + "_filtered";
-            debugDataInfo.getWriteBuffer().desired_torques[names] = right_desiredTorques_filtered(i);
-
-            if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
-            {
-                right_torque_targets.at(i)->torque = 0;
-            }
-            else
-            {
-                right_torque_targets.at(i)->torque = right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
-            }
-        }
-
-        smooth_startup = smooth_startup + 0.001 * (1.1  - smooth_startup);
-        smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
-        smooth_startup = (smooth_startup < 0) ?  0 : smooth_startup;
-
-
-
-        debugDataInfo.getWriteBuffer().left_realForce_x = left_forceInRoot(0);
-        debugDataInfo.getWriteBuffer().left_realForce_y = left_forceInRoot(1);
-        debugDataInfo.getWriteBuffer().left_realForce_z = left_forceInRoot(2);
-        debugDataInfo.getWriteBuffer().right_realForce_x = right_forceInRoot(0);
-        debugDataInfo.getWriteBuffer().right_realForce_y = right_forceInRoot(1);
-        debugDataInfo.getWriteBuffer().right_realForce_z = right_forceInRoot(2);
-
-        debugDataInfo.getWriteBuffer().left_force_error = left_force_error;
-        debugDataInfo.getWriteBuffer().right_force_error = right_force_error;
-
-
-        debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_x = left_tcpDesiredTorque_filtered(0);
-        debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_y = left_tcpDesiredTorque_filtered(1);
-        debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_z = left_tcpDesiredTorque_filtered(2);
-        debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_x = right_tcpDesiredTorque_filtered(0);
-        debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_y = right_tcpDesiredTorque_filtered(1);
-        debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_z = right_tcpDesiredTorque_filtered(2);
-        //        debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
-        //        debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
-        //        debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
-
-
-        //        debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
-        //        debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
-        //        debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
-
-        //        debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
-        //        debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
-        //        debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
-
-        //        debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
-        //        debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
-        //        debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
-
-        //        debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
-        //        debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
-        //        debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
-
-        debugDataInfo.commitWrite();
-
-        //    }
-        //    else
-        //    {
-        //        for (size_t i = 0; i < targets.size(); ++i)
-        //        {
-        //            targets.at(i)->torque = 0;
-
-        //        }
-        //    }
-
-
-
-    }
-
-    float DSRTBimanualController::deadzone(float input, float disturbance, float threshold)
-    {
-        if (input > 0)
-        {
-            input = input - disturbance;
-            input = (input < 0) ? 0 : input;
-            input = (input > threshold) ? threshold : input;
-        }
-        else if (input < 0)
-        {
-            input = input + disturbance;
-            input = (input > 0) ? 0 : input;
-            input = (input < -threshold) ? -threshold : input;
-        }
-
-
-        return input;
-    }
-
-    Eigen::Quaternionf DSRTBimanualController::quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1)
-    {
-        double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
-
-
-        Eigen::Quaternionf q1x = q1;
-        if (cosHalfTheta < 0)
-        {
-            q1x.w() = -q1.w();
-            q1x.x() = -q1.x();
-            q1x.y() = -q1.y();
-            q1x.z() = -q1.z();
-        }
-
-
-        if (fabs(cosHalfTheta) >= 1.0)
-        {
-            return q0;
-        }
-
-        double halfTheta = acos(cosHalfTheta);
-        double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
-
-
-        Eigen::Quaternionf result;
-        if (fabs(sinHalfTheta) < 0.001)
-        {
-            result.w() = (1 - t) * q0.w() + t * q1x.w();
-            result.x() = (1 - t) * q0.x() + t * q1x.x();
-            result.y() = (1 - t) * q0.y() + t * q1x.y();
-            result.z() = (1 - t) * q0.z() + t * q1x.z();
-
-        }
-        else
-        {
-            double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
-            double ratioB = sin(t * halfTheta) / sinHalfTheta;
-
-            result.w() = ratioA * q0.w() + ratioB * q1x.w();
-            result.x() = ratioA * q0.x() + ratioB * q1x.x();
-            result.y() = ratioA * q0.y() + ratioB * q1x.y();
-            result.z() = ratioA * q0.z() + ratioB * q1x.z();
-        }
-
-        return result;
-    }
-
-    void DSRTBimanualController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-
-        StringVariantBaseMap datafields;
-        auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        //    std::string nameJacobi = "jacobiori";
-        //    std::string nameJacobipos = "jacobipos";
-
-        //    std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
-        //    std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
-
-        //    for (size_t i = 0; i < jacobiVec.size(); ++i)
-        //    {
-        //        std::stringstream ss;
-        //        ss << i;
-        //        std::string name = nameJacobi + ss.str();
-        //        datafields[name] = new Variant(jacobiVec[i]);
-        //        std::string namepos = nameJacobipos + ss.str();
-        //        datafields[namepos] = new Variant(jacobiPos[i]);
-
-        //    }
-
-
-        datafields["left_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_x);
-        datafields["left_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_y);
-        datafields["left_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_z);
-        datafields["right_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_x);
-        datafields["right_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_y);
-        datafields["right_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_z);
-
-        datafields["left_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_x);
-        datafields["left_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_y);
-        datafields["left_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_z);
-        datafields["right_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_x);
-        datafields["right_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_y);
-        datafields["right_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_z);
-
-        datafields["left_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_force_error);
-        datafields["right_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_force_error);
-
-
-        datafields["Desired_Guard_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().desredZ);
-        datafields["Force_Error_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().force_error_z);
-        datafields["guardXYHighFrequency"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guardXYHighFrequency);
-        datafields["guard_mounting_correction_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_x);
-        datafields["guard_mounting_correction_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_y);
-        datafields["guard_mounting_correction_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_z);
-
-
-
-        //    datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
-        //    datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
-        //    datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
-
-        //    datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
-        //    datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
-        //    datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
-
-        //    datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
-        //    datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
-        //    datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
-
-        //    datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
-        //    datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
-        //    datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
-
-
-        //    datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
-        //    datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
-        //    datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
-
-
-        debugObs->setDebugChannel("DSBimanualControllerOutput", datafields);
-
-    }
-
-    void DSRTBimanualController::rtPreActivateController()
-    {
-
-    }
-
-    void DSRTBimanualController::rtPostDeactivateController()
-    {
-
-    }
-
-    void armarx::DSRTBimanualController::setToDefaultTarget(const Ice::Current&)
-    {
-        isDefaultTarget = true;
-    }
-}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
deleted file mode 100644
index 7a693af5524f0d7364de46a00746ad0d9e834f17..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
+++ /dev/null
@@ -1,548 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    DSController::ArmarXObjects::DSRTBimanualController
- * @author     Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#ifndef _ARMARX_LIB_DSController_DSRTBimanualController_H
-#define _ARMARX_LIB_DSController_DSRTBimanualController_H
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Tools/Gravity.h>
-
-#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
-#include "GMRDynamics.h"
-#include <ArmarXCore/util/json/JSONObject.h>
-
-#include "MathLib.h"
-//#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
-//#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
-
-
-namespace armarx
-{
-    class DSRTBimanualControllerControlData
-    {
-    public:
-        Eigen::Vector3f left_tcpDesiredLinearVelocity;
-        Eigen::Vector3f left_tcpDesiredAngularError;
-
-        Eigen::Vector3f right_tcpDesiredLinearVelocity;
-        Eigen::Vector3f right_tcpDesiredAngularError;
-
-        Eigen::Vector3f left_right_distanceInMeter;
-
-    };
-
-    using BimanualGMMPtr = std::shared_ptr<class GMRDynamics>;
-
-    struct BimanualGMRParameters
-    {
-        int K_gmm_;
-        int dim_;
-        std::vector<double> Priors_;
-        std::vector<double> Mu_;
-        std::vector<double> Sigma_;
-        std::vector<double> attractor_;
-        double dt_;
-    };
-
-
-    class BimanualGMMMotionGen
-    {
-    public:
-        BimanualGMMMotionGen() {}
-
-        BimanualGMMMotionGen(const std::string& fileName)
-        {
-            getGMMParamsFromJsonFile(fileName);
-        }
-
-        BimanualGMMPtr  leftarm_gmm;
-        BimanualGMMPtr  rightarm_gmm;
-
-        BimanualGMRParameters leftarm_gmmParas;
-        BimanualGMRParameters rightarm_gmmParas;
-
-        Eigen::Vector3f leftarm_Target;
-        Eigen::Vector3f rightarm_Target;
-
-        Eigen::Vector3f left_DS_DesiredVelocity;
-        Eigen::Vector3f right_DS_DesiredVelocity;
-        Eigen::Vector3f left_right_position_errorInMeter;
-
-
-
-        float scaling;
-        float v_max;
-
-        void getGMMParamsFromJsonFile(const std::string& fileName)
-        {
-            std::ifstream infile { fileName };
-            std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
-            //            StructuralJsonParser jsonParser(objDefs);
-            //            jsonParser.parse();
-            //            JPathNavigator jpnav(jsonParser.parsedJson);
-            //            float k = jpnav.selectSingleNode("left/K").asFloat();
-            //            ARMARX_INFO << "k: " << k ;
-            //            jpnav.selectSingleNode("left")
-            JSONObjectPtr json = new JSONObject();
-            json->fromString(objDefs);
-
-            //            leftarm_gmmParas.K_gmm_ = AbstractObjectSerializerPtr::dynamicCast<JSONObjectPtr>(json->getElement("left"))->getInt("K");
-            //            leftarm_gmmParas.dim_ = json->getElement("left")->getInt("dim");
-            //            boost::dynamic_pointer_cast<JSONObjectPtr>(json->getElement("left"))->getArray<double>("Priors", leftarm_gmmParas.Priors_);
-
-            //            json->getElement("left")->getArray<double>("Mu", leftarm_gmmParas.Mu_);
-            //            json->getElement("left")->getArray<double>("attractor", leftarm_gmmParas.attractor_);
-            //            json->getElement("left")->getArray<double>("Sigma", leftarm_gmmParas.Sigma_);
-
-            //            rightarm_gmmParas.K_gmm_ = json->getElement("right")->getInt("K");
-            //            rightarm_gmmParas.dim_ = json->getElement("right")->getInt("dim");
-            //            json->getElement("right")->getArray<double>("Priors", rightarm_gmmParas.Priors_);
-            //            json->getElement("right")->getArray<double>("Mu", rightarm_gmmParas.Mu_);
-            //            json->getElement("right")->getArray<double>("attractor", rightarm_gmmParas.attractor_);
-            //            json->getElement("right")->getArray<double>("Sigma", rightarm_gmmParas.Sigma_);
-
-
-            leftarm_gmmParas.K_gmm_ = json->getInt("leftK");
-            leftarm_gmmParas.dim_ = json->getInt("leftDim");
-            json->getArray<double>("leftPriors", leftarm_gmmParas.Priors_);
-            json->getArray<double>("leftMu", leftarm_gmmParas.Mu_);
-            json->getArray<double>("leftAttractor", leftarm_gmmParas.attractor_);
-            json->getArray<double>("leftSigma", leftarm_gmmParas.Sigma_);
-
-
-            rightarm_gmmParas.K_gmm_ = json->getInt("rightK");
-            rightarm_gmmParas.dim_ = json->getInt("rightDim");
-            json->getArray<double>("rightPriors", rightarm_gmmParas.Priors_);
-            json->getArray<double>("rightMu", rightarm_gmmParas.Mu_);
-            json->getArray<double>("rightAttractor", rightarm_gmmParas.attractor_);
-            json->getArray<double>("rightSigma", rightarm_gmmParas.Sigma_);
-
-
-            scaling = json->getDouble("Scaling");
-            v_max = json->getDouble("MaxVelocity");
-
-            leftarm_gmm.reset(new GMRDynamics(leftarm_gmmParas.K_gmm_, leftarm_gmmParas.dim_, leftarm_gmmParas.dt_, leftarm_gmmParas.Priors_, leftarm_gmmParas.Mu_, leftarm_gmmParas.Sigma_));
-            leftarm_gmm->initGMR(0, 2, 3, 5);
-
-            rightarm_gmm.reset(new GMRDynamics(rightarm_gmmParas.K_gmm_, rightarm_gmmParas.dim_, rightarm_gmmParas.dt_, rightarm_gmmParas.Priors_, rightarm_gmmParas.Mu_, rightarm_gmmParas.Sigma_));
-            rightarm_gmm->initGMR(0, 2, 3, 5);
-
-
-            //            if (leftarm_gmmParas.attractor_.size() < 3)
-            //            {
-            //                ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
-            //            }
-
-            //            if (rightarm_gmmParas.attractor_.size() < 3)
-            //            {
-            //                ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
-            //            }
-
-            std::cout << "line 162." << std::endl;
-
-
-            for (int i = 0; i < 3; ++i)
-            {
-                leftarm_Target(i) = leftarm_gmmParas.attractor_.at(i);
-                rightarm_Target(i) = rightarm_gmmParas.attractor_.at(i);
-            }
-
-            std::cout << "Finished GMM." << std::endl;
-
-        }
-
-
-        void updateDesiredVelocity(
-            const Eigen::Vector3f& leftarm_PositionInMeter,
-            const Eigen::Vector3f& rightarm_PositionInMeter,
-            float positionErrorToleranceInMeter,
-            float desiredZ,
-            float correction_x,
-            float correction_y,
-            float correction_z)
-        {
-            MathLib::Vector position_error;
-            position_error.Resize(3);
-
-            MathLib::Vector desired_vel;
-            desired_vel.Resize(3);
-
-
-
-            Eigen::Vector3f leftarm_Target_corrected = leftarm_Target;
-            Eigen::Vector3f rightarm_Target_corrected = rightarm_Target;
-
-            leftarm_Target_corrected(0) += correction_x;
-            rightarm_Target_corrected(0) += correction_x;
-
-            leftarm_Target_corrected(1) += correction_y;
-            rightarm_Target_corrected(1) += correction_y;
-
-            leftarm_Target_corrected(2)  = desiredZ + correction_z;
-            rightarm_Target_corrected(2) = desiredZ +  correction_z;
-
-
-
-            // for the left arm
-            Eigen::Vector3f leftarm_PositionError = leftarm_PositionInMeter - leftarm_Target_corrected;
-            if (leftarm_PositionError.norm() < positionErrorToleranceInMeter)
-            {
-                leftarm_PositionError.setZero();
-            }
-
-            for (int i = 0; i < 3; ++i)
-            {
-                position_error(i) = leftarm_PositionError(i);
-            }
-
-            desired_vel = leftarm_gmm->getVelocity(position_error);
-
-            Eigen::Vector3f leftarm_desired_vel;
-            leftarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
-
-            leftarm_desired_vel = scaling * leftarm_desired_vel;
-
-            float lenVec = leftarm_desired_vel.norm();
-
-            if (std::isnan(lenVec))
-            {
-                leftarm_desired_vel.setZero();
-            }
-
-            if (desiredZ < 1.5)
-            {
-                v_max = 0.2;
-            }
-            else
-            {
-                v_max = 0.5;
-            }
-
-            if (lenVec > v_max)
-            {
-                leftarm_desired_vel = (v_max / lenVec) * leftarm_desired_vel;
-            }
-
-            left_DS_DesiredVelocity = leftarm_desired_vel;
-
-
-            // for the right arm
-            Eigen::Vector3f rightarm_PositionError = rightarm_PositionInMeter - rightarm_Target_corrected;
-            if (rightarm_PositionError.norm() < positionErrorToleranceInMeter)
-            {
-                rightarm_PositionError.setZero();
-            }
-
-            for (int i = 0; i < 3; ++i)
-            {
-                position_error(i) = rightarm_PositionError(i);
-            }
-
-            desired_vel = rightarm_gmm->getVelocity(position_error);
-
-            Eigen::Vector3f rightarm_desired_vel;
-            rightarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
-
-            rightarm_desired_vel = scaling * rightarm_desired_vel;
-
-            lenVec = rightarm_desired_vel.norm();
-            if (std::isnan(lenVec))
-            {
-                rightarm_desired_vel.setZero();
-            }
-
-            if (lenVec > v_max)
-            {
-                rightarm_desired_vel = (v_max / lenVec) * rightarm_desired_vel;
-            }
-
-            right_DS_DesiredVelocity = rightarm_desired_vel;
-
-            left_right_position_errorInMeter = leftarm_PositionError - rightarm_PositionError;
-
-        }
-
-
-
-    };
-
-    using BimanualGMMMotionGenPtr = std::shared_ptr<BimanualGMMMotionGen>;
-
-
-
-
-    /**
-        * @defgroup Library-DSRTBimanualController DSRTBimanualController
-        * @ingroup DSController
-        * A description of the library DSRTBimanualController.
-        *
-        * @class DSRTBimanualController
-        * @ingroup Library-DSRTBimanualController
-        * @brief Brief description of class DSRTBimanualController.
-        *
-        * Detailed description of class DSRTBimanualController.
-        */
-
-    class DSRTBimanualController : public NJointControllerWithTripleBuffer<DSRTBimanualControllerControlData>, public DSBimanualControllerInterface
-    {
-
-        // ManagedIceObject interface
-    protected:
-        void onInitNJointController();
-        void onDisconnectNJointController();
-
-
-        void controllerRun();
-
-
-
-        // NJointControllerInterface interface
-    public:
-        using ConfigPtrT = DSRTBimanualControllerConfigPtr;
-
-        DSRTBimanualController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-
-        std::string getClassName(const Ice::Current&) const
-        {
-            return "DSRTBimanualController";
-        }
-
-        // NJointController interface
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        void setToDefaultTarget(const Ice::Current&);
-    private:
-
-        float deadzone(float currentValue, float targetValue, float threshold);
-        Eigen::Quaternionf quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1);
-
-        //        PeriodicTask<DSRTBimanualController>::pointer_type controllerTask;
-        BimanualGMMMotionGenPtr gmmMotionGenerator;
-        struct DSRTBimanualControllerSensorData
-        {
-            Eigen::Matrix4f left_tcpPose;
-            Eigen::Matrix4f right_tcpPose;
-
-            //            Eigen::Vector3f left_linearVelocity;
-            //            Eigen::Vector3f right_linearVelocity;
-
-            Eigen::Vector3f left_force;
-            Eigen::Vector3f right_force;
-
-
-            double currentTime;
-
-
-        };
-        TripleBuffer<DSRTBimanualControllerSensorData> controllerSensorData;
-
-        struct DSCtrlDebugInfo
-        {
-            float desredZ;
-            float force_error_z;
-            float guardXYHighFrequency;
-            float guard_mounting_correction_x;
-            float guard_mounting_correction_y;
-            float guard_mounting_correction_z;
-        };
-        TripleBuffer<DSCtrlDebugInfo> debugCtrlDataInfo;
-
-        struct DSRTDebugInfo
-        {
-            StringFloatDictionary desired_torques;
-            float desiredForce_x;
-            float desiredForce_y;
-            float desiredForce_z;
-            float tcpDesiredTorque_x;
-            float tcpDesiredTorque_y;
-            float tcpDesiredTorque_z;
-
-            float tcpDesiredAngularError_x;
-            float tcpDesiredAngularError_y;
-            float tcpDesiredAngularError_z;
-
-            float currentTCPAngularVelocity_x;
-            float currentTCPAngularVelocity_y;
-            float currentTCPAngularVelocity_z;
-
-            float currentTCPLinearVelocity_x;
-            float currentTCPLinearVelocity_y;
-            float currentTCPLinearVelocity_z;
-
-            // force torque sensor in root
-            float left_realForce_x;
-            float left_realForce_y;
-            float left_realForce_z;
-            float right_realForce_x;
-            float right_realForce_y;
-            float right_realForce_z;
-            float left_force_error;
-            float right_force_error;
-
-            float left_tcpDesiredTorque_x;
-            float left_tcpDesiredTorque_y;
-            float left_tcpDesiredTorque_z;
-            float right_tcpDesiredTorque_x;
-            float right_tcpDesiredTorque_y;
-            float right_tcpDesiredTorque_z;
-
-        };
-        TripleBuffer<DSRTDebugInfo> debugDataInfo;
-
-
-        std::vector<const SensorValue1DoFActuatorTorque*> left_torqueSensors;
-        std::vector<const SensorValue1DoFGravityTorque*> left_gravityTorqueSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> left_velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> left_positionSensors;
-
-        std::vector<const SensorValue1DoFActuatorTorque*> right_torqueSensors;
-        std::vector<const SensorValue1DoFGravityTorque*> right_gravityTorqueSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> right_velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> right_positionSensors;
-
-        const SensorValueForceTorque* leftForceTorque;
-        const SensorValueForceTorque* rightForceTorque;
-
-        std::vector<ControlTarget1DoFActuatorTorque*> left_torque_targets;
-        std::vector<ControlTarget1DoFActuatorTorque*> right_torque_targets;
-
-
-        VirtualRobot::RobotNodePtr left_arm_tcp;
-        VirtualRobot::RobotNodePtr right_arm_tcp;
-
-        VirtualRobot::RobotNodePtr left_sensor_frame;
-        VirtualRobot::RobotNodePtr right_sensor_frame;
-
-        VirtualRobot::DifferentialIKPtr left_ik;
-        Eigen::MatrixXf left_jacobip;
-        Eigen::MatrixXf left_jacobio;
-
-        VirtualRobot::DifferentialIKPtr right_ik;
-        Eigen::MatrixXf right_jacobip;
-        Eigen::MatrixXf right_jacobio;
-
-        Eigen::Quaternionf left_desiredQuaternion;
-        Eigen::Vector3f left_oldPosition;
-        Eigen::Matrix3f left_oldOrientation;
-
-        Eigen::Quaternionf right_desiredQuaternion;
-        Eigen::Vector3f right_oldPosition;
-        Eigen::Matrix3f right_oldOrientation;
-
-        Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
-        Eigen::Vector3f left_currentTCPAngularVelocity_filtered;
-
-        Eigen::VectorXf left_jointVelocity_filtered;
-        Eigen::VectorXf right_jointVelocity_filtered;
-
-        Eigen::VectorXf left_desiredTorques_filtered;
-        Eigen::VectorXf right_desiredTorques_filtered;
-
-
-        Eigen::Vector3f left_tcpDesiredTorque_filtered;
-        Eigen::Vector3f right_tcpDesiredTorque_filtered;
-
-        Eigen::Vector3f leftForceOffset;
-        Eigen::Vector3f rightForceOffset;
-
-        float left_contactForceZ_correction;
-        float right_contactForceZ_correction;
-
-        float smooth_startup;
-
-        DSRTBimanualControllerConfigPtr cfg;
-
-        Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
-        Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
-
-        float filterTimeConstant;
-
-        std::vector<std::string> left_jointNames;
-        std::vector<std::string> right_jointNames;
-
-        float posiKp;
-        float v_max;
-        std::vector<float> Damping;
-        float torqueLimit;
-        float null_torqueLimit;
-
-        float Coupling_stiffness;
-        float Coupling_force_limit;
-
-        float forward_gain;
-
-        float oriKp;
-        float oriDamping;
-
-        float nullspaceKp;
-        float nullspaceDamping;
-
-        float contactForce;
-
-        float guardTargetZUp;
-        float guardTargetZDown;
-        float guardDesiredZ;
-        float guard_mounting_correction_z;
-
-        float guardXYHighFrequency;
-        Eigen::Vector3f left_force_old;
-        Eigen::Vector3f right_force_old;
-
-        Eigen::VectorXf left_qnullspace;
-        Eigen::VectorXf right_qnullspace;
-
-        Eigen::Quaternionf left_oriUp;
-        Eigen::Quaternionf left_oriDown;
-        Eigen::Quaternionf right_oriUp;
-        Eigen::Quaternionf right_oriDown;
-
-        //        std::vector<BimanualGMMMotionGenPtr> BimanualGMMMotionGenList;
-
-
-        float forceFilterCoeff;
-
-        float positionErrorTolerance;
-        bool controllerStopRequested = false;
-        bool controllerRunFinished = false;
-
-        bool isDefaultTarget = true;
-
-        // NJointController interface
-    protected:
-        void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        // NJointController interface
-    protected:
-        void rtPreActivateController();
-        void rtPostDeactivateController();
-    };
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
deleted file mode 100644
index 2c9e75497c9730f7cc5b89a6bd67e12eb5a9852d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
+++ /dev/null
@@ -1,445 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    DSController::ArmarXObjects::DSRTController
- * @author     Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "DSRTController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<DSRTController> registrationControllerDSRTController("DSRTController");
-
-    void DSRTController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-
-        runTask("DSRTControllerTask", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-
-
-    }
-
-    void DSRTController::onDisconnectNJointController()
-    {
-
-    }
-
-
-    DSRTController::DSRTController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        DSControllerConfigPtr cfg = DSControllerConfigPtr::dynamicCast(config);
-        useSynchronizedRtRobot();
-        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
-        jointNames.clear();
-        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-        for (size_t i = 0; i < rns->getSize(); ++i)
-        {
-            std::string jointName = rns->getNode(i)->getName();
-            jointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            ARMARX_CHECK_EXPRESSION(ct);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            ARMARX_CHECK_EXPRESSION(sv);
-            auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
-            ARMARX_CHECK_EXPRESSION(casted_ct);
-            targets.push_back(casted_ct);
-
-            const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            if (!torqueSensor)
-            {
-                ARMARX_WARNING << "No Torque sensor available for " << jointName;
-            }
-            if (!gravityTorqueSensor)
-            {
-                ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
-            }
-
-            torqueSensors.push_back(torqueSensor);
-            gravityTorqueSensors.push_back(gravityTorqueSensor);
-            velocitySensors.push_back(velocitySensor);
-            positionSensors.push_back(positionSensor);
-        };
-        ARMARX_INFO << "Initialized " << targets.size() << " targets";
-        tcp =  rns->getTCP();
-        ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName;
-
-
-        ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-        DSRTControllerSensorData initSensorData;
-        initSensorData.tcpPose = tcp->getPoseInRootFrame();
-        initSensorData.currentTime = 0;
-        controllerSensorData.reinitAllBuffers(initSensorData);
-
-
-        oldPosition = tcp->getPositionInRootFrame();
-        oldOrientation = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
-
-        std::vector<float> desiredPositionVec = cfg->desiredPosition;
-        for (size_t i = 0; i < 3; ++i)
-        {
-            desiredPosition(i) = desiredPositionVec[i];
-        }
-        ARMARX_INFO << "ik reseted ";
-
-        std::vector<float> desiredQuaternionVec = cfg->desiredQuaternion;
-        ARMARX_CHECK_EQUAL(desiredQuaternionVec.size(), 4);
-
-        desiredQuaternion.x() = desiredQuaternionVec.at(0);
-        desiredQuaternion.y() = desiredQuaternionVec.at(1);
-        desiredQuaternion.z() = desiredQuaternionVec.at(2);
-        desiredQuaternion.w() = desiredQuaternionVec.at(3);
-
-
-
-        DSRTControllerControlData initData;
-        for (size_t i = 0; i < 3; ++i)
-        {
-            initData.tcpDesiredLinearVelocity(i) = 0;
-        }
-
-        for (size_t i = 0; i < 3; ++i)
-        {
-            initData.tcpDesiredAngularError(i) = 0;
-        }
-        reinitTripleBuffer(initData);
-
-        // initial filter
-        currentTCPLinearVelocity_filtered.setZero();
-        currentTCPAngularVelocity_filtered.setZero();
-        filterTimeConstant = cfg->filterTimeConstant;
-        posiKp = cfg->posiKp;
-        v_max = cfg->v_max;
-        posiDamping = cfg->posiDamping;
-        torqueLimit = cfg->torqueLimit;
-        oriKp = cfg->oriKp;
-        oriDamping  = cfg->oriDamping;
-
-
-        std::vector<float> qnullspaceVec = cfg->qnullspaceVec;
-
-        qnullspace.resize(qnullspaceVec.size());
-
-        for (size_t i = 0; i < qnullspaceVec.size(); ++i)
-        {
-            qnullspace(i) = qnullspaceVec[i];
-        }
-
-        nullspaceKp = cfg->nullspaceKp;
-        nullspaceDamping = cfg->nullspaceDamping;
-
-
-        //set GMM parameters ...
-        std::vector<std::string> gmmParamsFiles = cfg->gmmParamsFiles;
-        if (gmmParamsFiles.size() == 0)
-        {
-            ARMARX_ERROR << "no gmm found ... ";
-        }
-
-        gmmMotionGenList.clear();
-        float sumBelief = 0;
-        for (size_t i = 0; i < gmmParamsFiles.size(); ++i)
-        {
-            gmmMotionGenList.push_back(GMMMotionGenPtr(new GMMMotionGen(gmmParamsFiles.at(i))));
-            sumBelief += gmmMotionGenList[i]->belief;
-        }
-        ARMARX_CHECK_EQUAL(gmmMotionGenList.size(), 2);
-
-        dsAdaptorPtr.reset(new DSAdaptor(gmmMotionGenList, cfg->dsAdaptorEpsilon));
-        positionErrorTolerance = cfg->positionErrorTolerance;
-
-
-        ARMARX_INFO << "Initialization done";
-    }
-
-
-    void DSRTController::controllerRun()
-    {
-        if (!controllerSensorData.updateReadBuffer())
-        {
-            return;
-        }
-
-
-        Eigen::Matrix4f currentTCPPose = controllerSensorData.getReadBuffer().tcpPose;
-        Eigen::Vector3f realVelocity = controllerSensorData.getReadBuffer().linearVelocity;
-
-        Eigen::Vector3f currentTCPPositionInMeter;
-        currentTCPPositionInMeter << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
-        currentTCPPositionInMeter = 0.001 * currentTCPPositionInMeter;
-
-        dsAdaptorPtr->updateDesiredVelocity(currentTCPPositionInMeter, positionErrorTolerance);
-        Eigen::Vector3f tcpDesiredLinearVelocity = dsAdaptorPtr->totalDesiredVelocity;
-        dsAdaptorPtr->updateBelief(realVelocity);
-
-
-
-        float vecLen = tcpDesiredLinearVelocity.norm();
-        if (vecLen > v_max)
-        {
-
-            tcpDesiredLinearVelocity = tcpDesiredLinearVelocity / vecLen * v_max;
-        }
-
-        ARMARX_INFO << "tcpDesiredLinearVelocity: " << tcpDesiredLinearVelocity;
-
-        debugDataInfo.getWriteBuffer().belief0 = dsAdaptorPtr->task0_belief;
-        debugDataInfo.getWriteBuffer().belief1 = gmmMotionGenList[0]->belief;
-        debugDataInfo.getWriteBuffer().belief2 = gmmMotionGenList[1]->belief;
-        debugDataInfo.commitWrite();
-
-        Eigen::Vector3f tcpDesiredAngularError;
-        tcpDesiredAngularError << 0, 0, 0;
-
-        Eigen::Matrix3f currentOrientation = currentTCPPose.block<3, 3>(0, 0);
-        Eigen::Matrix3f desiredOrientation = desiredQuaternion.normalized().toRotationMatrix();
-        Eigen::Matrix3f orientationError = currentOrientation * desiredOrientation.inverse();
-        Eigen::Quaternionf diffQuaternion(orientationError);
-        Eigen::AngleAxisf angleAxis(diffQuaternion);
-        tcpDesiredAngularError = angleAxis.angle() * angleAxis.axis();
-
-        getWriterControlStruct().tcpDesiredLinearVelocity = tcpDesiredLinearVelocity;
-        getWriterControlStruct().tcpDesiredAngularError = tcpDesiredAngularError;
-
-        writeControlStruct();
-    }
-
-
-    void DSRTController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-        if (deltaT != 0)
-        {
-
-            Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
-
-
-            Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-            Eigen::VectorXf qpos;
-            Eigen::VectorXf qvel;
-            qpos.resize(positionSensors.size());
-            qvel.resize(velocitySensors.size());
-
-            int jointDim = positionSensors.size();
-
-            for (size_t i = 0; i < velocitySensors.size(); ++i)
-            {
-                qpos(i) = positionSensors[i]->position;
-                qvel(i) = velocitySensors[i]->velocity;
-            }
-
-            Eigen::VectorXf tcptwist = jacobi * qvel;
-
-            Eigen::Vector3f currentTCPLinearVelocity;
-            currentTCPLinearVelocity << 0.001 * tcptwist(0),  0.001 * tcptwist(1), 0.001 * tcptwist(2);
-            double filterFactor = deltaT / (filterTimeConstant + deltaT);
-            currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
-
-            controllerSensorData.getWriteBuffer().tcpPose = currentTCPPose;
-            controllerSensorData.getWriteBuffer().currentTime += deltaT;
-            controllerSensorData.getWriteBuffer().linearVelocity = currentTCPLinearVelocity_filtered;
-            controllerSensorData.commitWrite();
-
-
-            Eigen::Vector3f currentTCPAngularVelocity;
-            currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
-            //        // calculate linear velocity
-            //        Eigen::Vector3f currentTCPPosition;
-            //        currentTCPPosition << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
-            //        Eigen::Vector3f currentTCPLinearVelocity_raw = (currentTCPPosition - oldPosition) / deltaT;
-            //        oldPosition = currentTCPPosition;
-
-            //        // calculate angular velocity
-            //        Eigen::Matrix3f currentTCPOrientation = currentTCPPose.block<3, 3>(0, 0);
-            //        Eigen::Matrix3f currentTCPDiff = currentTCPOrientation * oldOrientation.inverse();
-            //        Eigen::AngleAxisf currentAngleAxisDiff(currentTCPDiff);
-            //        Eigen::Vector3f currentTCPAngularVelocity_raw = currentAngleAxisDiff.angle() * currentAngleAxisDiff.axis();
-            //        oldOrientation = currentTCPOrientation;
-            //        currentTCPAngularVelocity_filtered = (1 - filterFactor) * currentTCPAngularVelocity_filtered + filterFactor * currentTCPAngularVelocity_raw;
-
-
-            Eigen::Vector3f tcpDesiredLinearVelocity = rtGetControlStruct().tcpDesiredLinearVelocity;
-            Eigen::Vector3f tcpDesiredAngularError = rtGetControlStruct().tcpDesiredAngularError;
-
-            // calculate desired tcp force
-            Eigen::Vector3f tcpDesiredForce = -posiDamping * (currentTCPLinearVelocity_filtered - tcpDesiredLinearVelocity);
-
-            // calculate desired tcp torque
-            Eigen::Vector3f tcpDesiredTorque = - oriKp * tcpDesiredAngularError - oriDamping * currentTCPAngularVelocity;
-
-            Eigen::Vector6f tcpDesiredWrench;
-            tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
-
-            // calculate desired joint torque
-            Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
-
-            float lambda = 2;
-            Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
-
-            Eigen::VectorXf nullqerror = qpos - qnullspace;
-
-            for (int i = 0; i < nullqerror.rows(); ++i)
-            {
-                if (fabs(nullqerror(i)) < 0.09)
-                {
-                    nullqerror(i) = 0;
-                }
-            }
-            Eigen::VectorXf nullspaceTorque = - nullspaceKp * nullqerror - nullspaceDamping * qvel;
-
-            Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
-            for (size_t i = 0; i < targets.size(); ++i)
-            {
-                float desiredTorque = jointDesiredTorques(i);
-
-                desiredTorque = (desiredTorque >  torqueLimit) ? torqueLimit : desiredTorque;
-                desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
-                debugDataInfo.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
-
-                targets.at(i)->torque = desiredTorque; // targets.at(i)->velocity = desiredVelocity;
-            }
-
-
-            debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
-            debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
-            debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
-
-
-            debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
-            debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
-            debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
-
-            debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
-            debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
-            debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
-
-            debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
-            debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
-            debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
-
-            debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
-            debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
-            debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
-
-            debugDataInfo.commitWrite();
-
-        }
-        else
-        {
-            for (size_t i = 0; i < targets.size(); ++i)
-            {
-                targets.at(i)->torque = 0;
-
-            }
-        }
-
-
-
-    }
-
-    void DSRTController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-
-        StringVariantBaseMap datafields;
-        auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        //    std::string nameJacobi = "jacobiori";
-        //    std::string nameJacobipos = "jacobipos";
-
-        //    std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
-        //    std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
-
-        //    for (size_t i = 0; i < jacobiVec.size(); ++i)
-        //    {
-        //        std::stringstream ss;
-        //        ss << i;
-        //        std::string name = nameJacobi + ss.str();
-        //        datafields[name] = new Variant(jacobiVec[i]);
-        //        std::string namepos = nameJacobipos + ss.str();
-        //        datafields[namepos] = new Variant(jacobiPos[i]);
-
-        //    }
-
-
-
-        datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
-        datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
-        datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
-
-        datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
-        datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
-        datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
-
-        datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
-        datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
-        datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
-
-        datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
-        datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
-        datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
-
-
-        datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
-        datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
-        datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
-
-        datafields["belief_0"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief0);
-        datafields["belief_1"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief1);
-        datafields["belief_2"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief2);
-
-        debugObs->setDebugChannel("DSControllerOutput", datafields);
-
-    }
-
-    void DSRTController::rtPreActivateController()
-    {
-
-    }
-
-    void DSRTController::rtPostDeactivateController()
-    {
-
-    }
-}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.h b/source/RobotAPI/libraries/DSControllers/DSRTController.h
deleted file mode 100644
index 3ac02e59e58a27e44e0dc59a9682286dc8c0f938..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSRTController.h
+++ /dev/null
@@ -1,718 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    DSController::ArmarXObjects::DSRTController
- * @author     Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#ifndef _ARMARX_LIB_DSController_DSRTController_H
-#define _ARMARX_LIB_DSController_DSRTController_H
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Tools/Gravity.h>
-
-#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
-#include "GMRDynamics.h"
-#include <ArmarXCore/util/json/JSONObject.h>
-
-#include "MathLib.h"
-
-namespace armarx
-{
-    class DSRTControllerControlData
-    {
-    public:
-        Eigen::Vector3f tcpDesiredLinearVelocity;
-        Eigen::Vector3f tcpDesiredAngularError;
-    };
-
-    using GMMPtr = std::shared_ptr<GMRDynamics>;
-
-    struct GMRParameters
-    {
-        int K_gmm_;
-        int dim_;
-        std::vector<double> Priors_;
-        std::vector<double> Mu_;
-        std::vector<double> Sigma_;
-        std::vector<double> attractor_;
-        double dt_;
-    };
-
-
-    class GMMMotionGen
-    {
-    public:
-        GMMMotionGen() {}
-
-        GMMMotionGen(const std::string& fileName)
-        {
-            getGMMParamsFromJsonFile(fileName);
-        }
-
-        GMMPtr  gmm;
-        GMRParameters gmmParas;
-        Eigen::Vector3f desiredDefaultTarget;
-        float scaling;
-
-        float belief;
-        float v_max;
-
-        void getGMMParamsFromJsonFile(const std::string& fileName)
-        {
-            std::ifstream infile { fileName };
-            std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
-            JSONObjectPtr json = new JSONObject();
-            json->fromString(objDefs);
-
-
-            gmmParas.K_gmm_ = json->getInt("K");
-            gmmParas.dim_ = json->getInt("dim");
-            json->getArray<double>("Priors", gmmParas.Priors_);
-            json->getArray<double>("Mu", gmmParas.Mu_);
-            json->getArray<double>("attractor", gmmParas.attractor_);
-            json->getArray<double>("Sigma", gmmParas.Sigma_);
-
-            scaling = json->getDouble("Scaling");
-            belief = json->getDouble("InitBelief");
-            belief = 0;
-            v_max = json->getDouble("MaxVelocity");
-
-            gmm.reset(new GMRDynamics(gmmParas.K_gmm_, gmmParas.dim_, gmmParas.dt_, gmmParas.Priors_, gmmParas.Mu_, gmmParas.Sigma_));
-            gmm->initGMR(0, 2, 3, 5);
-
-            if (gmmParas.attractor_.size() < 3)
-            {
-                ARMARX_ERROR << "attractor in json file should be 6 dimension vector ... ";
-            }
-
-            for (int i = 0; i < 3; ++i)
-            {
-                desiredDefaultTarget(i) = gmmParas.attractor_.at(i);
-            }
-        }
-
-        void updateDesiredVelocity(const Eigen::Vector3f& currentPositionInMeter, float positionErrorToleranceInMeter)
-        {
-            Eigen::Vector3f PositionError = currentPositionInMeter - desiredDefaultTarget;
-            if (PositionError.norm() < positionErrorToleranceInMeter)
-            {
-                PositionError.setZero();
-            }
-
-            MathLib::Vector position_error;
-            position_error.Resize(3);
-
-            for (int i = 0; i < 3; ++i)
-            {
-                position_error(i) = PositionError(i);
-            }
-
-            MathLib::Vector desired_vel;
-            desired_vel.Resize(3);
-            desired_vel = gmm->getVelocity(position_error);
-
-            Eigen::Vector3f tcpDesiredLinearVelocity;
-            tcpDesiredLinearVelocity << desired_vel(0), desired_vel(1), desired_vel(2);
-
-            currentDesiredVelocity = scaling * tcpDesiredLinearVelocity;
-
-
-            float lenVec = tcpDesiredLinearVelocity.norm();
-            if (std::isnan(lenVec))
-            {
-                tcpDesiredLinearVelocity.setZero();
-            }
-
-            if (lenVec > v_max)
-            {
-                tcpDesiredLinearVelocity = (v_max / lenVec) * tcpDesiredLinearVelocity;
-            }
-        }
-
-
-
-        Eigen::Vector3f currentDesiredVelocity;
-    };
-
-    using GMMMotionGenPtr = std::shared_ptr<GMMMotionGen>;
-
-    class DSAdaptor
-    {
-    public:
-        float task0_belief;
-        float epsilon;
-        DSAdaptor() {}
-
-        DSAdaptor(std::vector<GMMMotionGenPtr> gmmMotionGenList, float epsilon)
-        {
-            task0_belief = 1;
-            this->gmmMotionGenList = gmmMotionGenList;
-
-            ARMARX_INFO << "epsilon: " << epsilon;
-            this->epsilon = epsilon;
-
-            totalDesiredVelocity.setZero();
-        }
-
-        Eigen::Vector3f totalDesiredVelocity;
-        std::vector<GMMMotionGenPtr> gmmMotionGenList;
-
-
-        void updateDesiredVelocity(const Eigen::Vector3f& currentPositionInMeter, float positionErrorToleranceInMeter)
-        {
-            totalDesiredVelocity.setZero();
-            for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
-            {
-                gmmMotionGenList[i]->updateDesiredVelocity(currentPositionInMeter, positionErrorToleranceInMeter);
-                totalDesiredVelocity +=  gmmMotionGenList[i]->belief * gmmMotionGenList[i]->currentDesiredVelocity;
-            }
-        }
-
-        void updateBelief(const Eigen::Vector3f& realVelocity)
-        {
-            std::vector<float> beliefUpdate;
-            beliefUpdate.resize(gmmMotionGenList.size());
-
-            float nullInnerSimilarity = 0;
-            for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
-            {
-
-                GMMMotionGenPtr currentGMM = gmmMotionGenList[i];
-
-                float belief = currentGMM->belief;
-                Eigen::Vector3f OtherTasks = totalDesiredVelocity - belief * currentGMM->currentDesiredVelocity;
-                float innerSimilarity = 2 * OtherTasks.dot(currentGMM->currentDesiredVelocity);
-                float outerDisSimilarity = (realVelocity - currentGMM->currentDesiredVelocity).squaredNorm();
-
-                if (innerSimilarity > nullInnerSimilarity)
-                {
-                    nullInnerSimilarity = innerSimilarity;
-                }
-
-                beliefUpdate[i] = - outerDisSimilarity - innerSimilarity;
-
-
-            }
-
-
-
-
-            float nullOuterSimilarity = realVelocity.squaredNorm();
-            float zeroTaskRawBeliefUpdate = - nullInnerSimilarity - nullOuterSimilarity;
-
-            if (zeroTaskRawBeliefUpdate < 0.2)
-            {
-                zeroTaskRawBeliefUpdate -= 1000;
-            }
-
-
-            beliefUpdate.insert(beliefUpdate.begin(), zeroTaskRawBeliefUpdate);
-
-            WinnerTakeAll(beliefUpdate);
-            task0_belief += epsilon * beliefUpdate[0];
-            if (task0_belief > 1)
-            {
-                task0_belief = 1;
-            }
-            else if (task0_belief < 0)
-            {
-                task0_belief = 0;
-            }
-
-            float beliefSum = task0_belief;
-
-            for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
-            {
-                gmmMotionGenList[i]->belief += epsilon * beliefUpdate[i + 1];
-
-
-                if (gmmMotionGenList[i]->belief > 1)
-                {
-                    gmmMotionGenList[i]->belief = 1;
-                }
-                else if (gmmMotionGenList[i]->belief < 0)
-                {
-                    gmmMotionGenList[i]->belief = 0;
-                }
-
-                beliefSum += gmmMotionGenList[i]->belief;
-
-            }
-
-            for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
-            {
-                gmmMotionGenList[i]->belief /= beliefSum;
-            }
-
-            task0_belief /= beliefSum;
-        }
-
-    private:
-
-        void WinnerTakeAll(std::vector<float>& UpdateBeliefs_)
-        {
-            //            std::fill(UpdateBeliefs_.begin(), UpdateBeliefs_.end(), 0);
-
-            size_t winner_index = 0;
-
-            for (size_t i = 1; i < UpdateBeliefs_.size(); i++)
-            {
-                if (UpdateBeliefs_[i] > UpdateBeliefs_[winner_index])
-                {
-                    winner_index = i;
-                }
-            }
-
-            float winner_belief = task0_belief;
-
-            if (winner_index != 0)
-            {
-                winner_belief = gmmMotionGenList[winner_index - 1]->belief;
-            }
-
-            if (winner_belief == 1)
-            {
-                return;
-            }
-
-            int runnerUp_index = 0;
-
-            if (winner_index == 0)
-            {
-                runnerUp_index = 1;
-            }
-
-            for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
-            {
-                if (i ==  winner_index)
-                {
-                    continue;
-                }
-
-                if (UpdateBeliefs_[i] > UpdateBeliefs_[runnerUp_index])
-                {
-                    runnerUp_index = i;
-                }
-            }
-
-            float offset = 0.5 * (UpdateBeliefs_[winner_index] + UpdateBeliefs_[runnerUp_index]);
-
-            for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
-            {
-                UpdateBeliefs_[i] -= offset;
-            }
-
-            float UpdateSum = 0;
-
-            for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
-            {
-                float belief = task0_belief;
-                if (i != 0)
-                {
-                    belief = gmmMotionGenList[i - 1]->belief;
-                }
-
-                if (belief != 0 || UpdateBeliefs_[i] > 0)
-                {
-                    UpdateSum += UpdateBeliefs_[i];
-                }
-            }
-
-            UpdateBeliefs_[winner_index] -= UpdateSum;
-        }
-    };
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-    using DSAdaptorPtr = std::shared_ptr<DSAdaptor>;
-
-
-
-    /**
-        * @defgroup Library-DSRTController DSRTController
-        * @ingroup DSController
-        * A description of the library DSRTController.
-        *
-        * @class DSRTController
-        * @ingroup Library-DSRTController
-        * @brief Brief description of class DSRTController.
-        *
-        * Detailed description of class DSRTController.
-        */
-
-    class DSRTController : public NJointControllerWithTripleBuffer<DSRTControllerControlData>, public DSControllerInterface
-    {
-
-        // ManagedIceObject interface
-    protected:
-        void onInitNJointController();
-        void onDisconnectNJointController();
-
-
-        void controllerRun();
-
-
-
-        // NJointControllerInterface interface
-    public:
-        using ConfigPtrT = DSControllerConfigPtr;
-
-        DSRTController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-
-        std::string getClassName(const Ice::Current&) const
-        {
-            return "DSRTController";
-        }
-
-        // NJointController interface
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-    private:
-        //        PeriodicTask<DSRTController>::pointer_type controllerTask;
-
-        struct DSRTControllerSensorData
-        {
-            Eigen::Matrix4f tcpPose;
-            double currentTime;
-
-            Eigen::Vector3f linearVelocity;
-
-        };
-        TripleBuffer<DSRTControllerSensorData> controllerSensorData;
-
-        struct DSRTDebugInfo
-        {
-            StringFloatDictionary desired_torques;
-            float desiredForce_x;
-            float desiredForce_y;
-            float desiredForce_z;
-            float tcpDesiredTorque_x;
-            float tcpDesiredTorque_y;
-            float tcpDesiredTorque_z;
-
-            float tcpDesiredAngularError_x;
-            float tcpDesiredAngularError_y;
-            float tcpDesiredAngularError_z;
-
-            float currentTCPAngularVelocity_x;
-            float currentTCPAngularVelocity_y;
-            float currentTCPAngularVelocity_z;
-
-            float currentTCPLinearVelocity_x;
-            float currentTCPLinearVelocity_y;
-            float currentTCPLinearVelocity_z;
-
-            float belief0;
-            float belief1;
-            float belief2;
-        };
-        TripleBuffer<DSRTDebugInfo> debugDataInfo;
-
-        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
-        std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-
-        std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
-
-        VirtualRobot::RobotNodePtr tcp;
-
-        VirtualRobot::DifferentialIKPtr ik;
-        Eigen::MatrixXf jacobip;
-        Eigen::MatrixXf jacobio;
-
-        Eigen::Vector3f desiredPosition;
-
-        Eigen::Quaternionf desiredQuaternion;
-        Eigen::Vector3f oldPosition;
-
-        Eigen::Matrix3f oldOrientation;
-
-        Eigen::Vector3f currentTCPLinearVelocity_filtered;
-        Eigen::Vector3f currentTCPAngularVelocity_filtered;
-
-        float filterTimeConstant;
-
-        std::vector<std::string> jointNames;
-
-        float posiKp;
-        float v_max;
-        float posiDamping;
-        float torqueLimit;
-
-        float oriKp;
-        float oriDamping;
-
-        float nullspaceKp;
-        float nullspaceDamping;
-
-        Eigen::VectorXf qnullspace;
-
-        std::vector<GMMMotionGenPtr> gmmMotionGenList;
-
-        DSAdaptorPtr dsAdaptorPtr;
-
-        float positionErrorTolerance;
-
-
-        // NJointController interface
-    protected:
-        void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        // NJointController interface
-    protected:
-        void rtPreActivateController();
-        void rtPostDeactivateController();
-    };
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp b/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp
deleted file mode 100644
index 9e3f7aa0e0ddd0b49cfffebb54447441e7e42b21..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- * GMRDynamics.cpp
- *
- *  Created on: Nov 20, 2011
- *      Author: Seungsu KIM
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-#include "GMRDynamics.h"
-
-
-GMRDynamics::GMRDynamics(int nStates, int nVar, double delta_t, const char* f_mu, const char* f_sigma, const char* f_prio)
-{
-    this->delta_t = delta_t;
-    GMM = new Gaussians(nStates, nVar, f_mu, f_sigma, f_prio);
-}
-
-GMRDynamics::GMRDynamics(int nStates, int nVar, double delta_t, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec)
-{
-    this->delta_t = delta_t;
-    GMM = new Gaussians(nStates, nVar, pri_vec, mu_vec, sig_vec);
-}
-
-
-void GMRDynamics::initGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
-{
-    GMM->InitFastGMR(first_inindex, last_inindex, first_outindex, last_outindex);
-
-    gDim = last_inindex - first_inindex + 1;
-    if (gDim != static_cast<unsigned int>(last_outindex - first_outindex + 1))
-    {
-        cout << "dynamics dimension is not matching" << endl;
-    }
-
-    gXi.Resize(gDim);
-    target.Resize(gDim);
-
-    gXi.Zero();
-    target.Zero();
-}
-
-void GMRDynamics::setStateTarget(MathLib::Vector state, MathLib::Vector target)
-{
-    setTarget(target);
-    setState(state);
-}
-
-void GMRDynamics::setTarget(MathLib::Vector target, double target_t)
-{
-    this->target_t = target_t;
-
-    //gXi += (this->target - target);
-    this->target = target;
-}
-
-MathLib::Vector GMRDynamics::getTarget(void)
-{
-    return target;
-}
-
-double GMRDynamics::getTargetT(void)
-{
-    return target_t;
-}
-
-void GMRDynamics::setState(MathLib::Vector state)
-{
-    gXi = state;
-}
-
-MathLib::Vector GMRDynamics::getState(void)
-{
-    return gXi;
-}
-
-void GMRDynamics::setCurrentTime(double current_t)
-{
-    this->current_t = current_t;
-}
-
-double GMRDynamics::getCurrentTime(void)
-{
-    return current_t;
-}
-
-MathLib::Vector GMRDynamics::getVelocity(MathLib::Vector x)
-{
-    return GMM->Regression(x);
-
-
-}
-
-
-MathLib::Vector GMRDynamics::getNextState(void)
-{
-    return getNextState(1.0);
-}
-
-MathLib::Vector GMRDynamics::getNextState(double lamda)
-{
-    // target time
-    target_t -= (delta_t* lamda);
-
-    gXi += (getVelocity(gXi - target) * (delta_t* lamda));
-
-    return gXi;
-}
-
-double GMRDynamics::getReachingTime(double lamda)
-{
-    unsigned int frame = 0;
-    unsigned int li = 0;
-    MathLib::Vector xi(3);
-    xi.Set(gXi);
-
-    for (frame = 0; frame < REACHING_ITERATION_MAX; frame++)
-    {
-        for (li = 0; li < INTEGRATION_L; li++)
-        {
-            xi += getVelocity(xi - target) * delta_t / (double)INTEGRATION_L * lamda;
-
-            if ((xi - target).Norm() < GMR_ERROR_TOLERANCE)
-            {
-                return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
-            }
-        }
-    }
-    return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
-}
-
diff --git a/source/RobotAPI/libraries/DSControllers/GMRDynamics.h b/source/RobotAPI/libraries/DSControllers/GMRDynamics.h
deleted file mode 100644
index ca461a6806e0fb2f243ab313b84a8c82f60d1bca..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/GMRDynamics.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
- * GMRDynamics.h
- *
- *  Created on: Nov 20, 2011
- *      Author: Seungsu KIM
- */
-
-#ifndef __GMRDYNAMICS_H__
-#define __GMRDYNAMICS_H__
-
-#include "Gaussians.h"
-
-#define GMR_ERROR_TOLERANCE 0.001
-#define INTEGRATION_L 5
-#define REACHING_ITERATION_MAX 500
-#define REAL_MIN (1e-30)
-
-// GMR Dynamics
-class GMRDynamics
-{
-private:
-    Gaussians* GMM;
-
-    double delta_t;
-    double target_t;
-    double current_t;
-
-    MathLib::Vector gXi;
-    MathLib::Vector target;
-    unsigned int gDim;
-
-public:
-    GMRDynamics(int nStates, int nVar, double delta_t, const char* f_mu, const char* f_sigma, const char* f_prio);
-    GMRDynamics(int nStates, int nVar, double delta_t, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec);
-
-    void initGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex);
-
-    void   setStateTarget(MathLib::Vector state, MathLib::Vector target);
-    void   setTarget(MathLib::Vector target, double target_t = -1.0);
-    MathLib::Vector getTarget(void);
-    double getTargetT(void);
-    void   setState(MathLib::Vector state);
-    MathLib::Vector getState(void);
-    void   setCurrentTime(double current_t);
-    double getCurrentTime(void);
-
-    MathLib::Vector getVelocity(MathLib::Vector x);
-
-    MathLib::Vector getNextState(void);
-    MathLib::Vector getNextState(double lamda);
-    double getReachingTime(double lamda);
-};
-
-
-
-#endif //__GMRDYNAMICS_H__
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.cpp b/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
deleted file mode 100644
index 7109ed73d21280484cccd5174cf83a84ccbdcf46..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
+++ /dev/null
@@ -1,561 +0,0 @@
-/*
- * Gaussians.cpp
- *
- *  Created on: Nov 19, 2011
- *      Author: Seungsu KIM
- */
-
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-#include "Gaussians.h"
-
-using namespace std;
-/*
-Gaussians::Gaussians(GMMs *model)
-{
-    this->model.nbStates = model->nbStates;
-    this->model.nbDim    = model->nbDim;
-
-    this->model.States = (GMMState  *)malloc(model->nbStates*sizeof(GMMState) );
-
-    for(int s=0; s<model->nbStates; s++ ){
-        this->model.States[s].Mu    = model->GMMState[s].Mu;
-        this->model.States[s].Sigma = model->GMMState[s].Sigma;
-        this->model.States[s].Prio  = model->GMMState[s].Prio;
-    }
-}
-*/
-
-Gaussians::Gaussians(const char* f_mu, const char* f_sigma, const char* f_prio)
-{
-    int s, i, j;
-    int nbStates;
-    int nbDim;
-
-    MathLib::Matrix fMatrix;
-    fMatrix.Load(f_prio);
-    if (fMatrix(0, fMatrix.ColumnSize() - 1) > 0.0)
-    {
-        nbStates = fMatrix.ColumnSize();
-    }
-    else
-    {
-        nbStates = fMatrix.ColumnSize() - 1;
-    }
-
-    for (s = 0; s < nbStates; s++)
-    {
-        model.States[s].Prio = fMatrix(0, s);
-    }
-
-    fMatrix.Load(f_mu);
-    nbDim = fMatrix.RowSize();
-    model.nbStates = nbStates;
-    model.nbDim    = nbDim;
-
-
-    for (s = 0; s < nbStates; s++)
-    {
-        model.States[s].Mu.Resize(nbDim);
-        model.States[s].Sigma.Resize(nbDim, nbDim);
-    }
-
-    for (s = 0; s < nbStates; s++)
-    {
-        model.States[s].Mu = fMatrix.GetColumn(s);
-    }
-
-    fMatrix.Load(f_sigma);
-    j = 0;
-    for (s = 0; s < nbStates; s++)
-    {
-        for (i = 0; i < nbDim; i++)
-        {
-            model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
-            j++;
-        }
-    }
-}
-
-Gaussians::Gaussians(int nbStates, int nbDim, const char* f_mu, const char* f_sigma, const char* f_prio)
-{
-
-    int s, i, j;
-
-    model.nbStates = nbStates;
-    model.nbDim    = nbDim;
-
-    for (s = 0; s < nbStates; s++)
-    {
-        model.States[s].Mu.Resize(nbDim);
-        model.States[s].Sigma.Resize(nbDim, nbDim);
-    }
-
-    MathLib::Matrix fMatrix(nbDim, nbStates);
-    fMatrix.Load(f_mu);
-    for (s = 0; s < nbStates; s++)
-    {
-        model.States[s].Mu = fMatrix.GetColumn(s);
-    }
-
-    fMatrix.Resize(nbStates * nbDim, nbDim);
-    fMatrix.Load(f_sigma);
-    j = 0;
-    for (s = 0; s < nbStates; s++)
-    {
-        for (i = 0; i < nbDim; i++)
-        {
-            model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
-            j++;
-        }
-    }
-
-    fMatrix.Resize(1, nbStates);
-    fMatrix.Load(f_prio);
-    MathLib::Vector fVector(nbStates);
-    for (s = 0; s < nbStates; s++)
-    {
-        model.States[s].Prio = fMatrix(0, s);
-    }
-
-}
-
-Gaussians::Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec)
-{
-
-    model.nbStates = nbStates;
-    model.nbDim    = nbDim;
-
-    for (int s = 0; s < nbStates; s++)
-    {
-        model.States[s].Mu.Resize(nbDim);
-        model.States[s].Sigma.Resize(nbDim, nbDim);
-    }
-
-    for (int s = 0; s < nbStates; s++)
-    {
-        model.States[s].Prio = pri_vec[s];
-    }
-    // cout << endl << "Printing the constructed Priors" << endl;
-    // for ( int s = 0; s < nbStates; s++ ) {
-    //  cout << model.States[s].Prio  << "\t";
-    // }
-    // cout << endl;
-
-
-    for (int s = 0; s < nbStates; s++)
-    {
-        for (int d = 0; d < nbDim; d++)
-        {
-            model.States[s].Mu[d] = mu_vec[s * nbDim + d];
-        }
-    }
-
-
-    // cout << endl << "Printing the constructed Mu" << endl;
-    // for ( int s = 0; s < nbStates; s++ ) {
-    //  for (int d = 0; d < nbDim; d++) {
-    //      cout << model.States[s].Mu[d]  << "\t";
-    //  }
-    //  cout << endl;
-    // }
-
-    for (int s = 0; s < nbStates; s++)
-    {
-        for (int row = 0; row < nbDim; row++)
-        {
-            for (int col = 0; col < nbDim; col++)
-            {
-                int ind = s * nbDim * nbDim + row * nbDim + col;
-                model.States[s].Sigma(row, col) = sig_vec[ind];
-            }
-        }
-    }
-
-
-    // cout << endl << "Printing the constructed Sigma" << endl;
-    // for ( int s = 0; s < nbStates; s++ ) {
-    //  for (int row = 0; row < nbDim; row++) {
-    //      for (int col = 0; col < nbDim; col++) {
-    //          cout << model.States[s].Sigma(row, col) << "\t";
-    //      }
-    //      cout <<endl;
-    //  }
-    //  cout << endl;
-    // }
-
-
-
-
-}
-
-
-
-
-void Gaussians::setGMMs(GMMs* model)
-{
-    for (unsigned int s = 0; s < model->nbStates; s++)
-    {
-        this->model.States[s].Mu    = model->States[s].Mu;
-        this->model.States[s].Sigma = model->States[s].Sigma;
-        this->model.States[s].Prio  = model->States[s].Prio;
-    }
-}
-
-
-void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
-{
-    double det;
-    int nbIN = last_inindex - first_inindex + 1;
-
-    for (unsigned int s = 0; s < model.nbStates; s++)
-    {
-        gmmpinv[s].MuI.Resize(nbIN);
-        gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
-        gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
-    }
-
-    for (unsigned int s = 0; s < model.nbStates; s++)
-    {
-        for (int i = first_inindex; i <= last_inindex; i++)
-        {
-            gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
-        }
-        for (int i = first_inindex; i <= last_inindex; i++)
-            for (int j = first_inindex; j <= last_inindex; j++)
-            {
-                gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
-            }
-
-        gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
-        //gmmpinv[s].SigmaIIInv = gmmpinv[s].SigmaII.Inverse();
-        //gmmpinv[s].SigmaII.Inverse(&det);
-        if (det < 0)
-        {
-            det = 1e-30;
-        }
-        gmmpinv[s].detSigmaII = det;
-
-    }
-
-    nbDimI = last_inindex - first_inindex + 1;
-    gfDiff.Resize(nbDimI);
-    gfDiffp.Resize(nbDimI);
-    gDer.Resize(nbDimI);
-
-}
-
-double Gaussians::GaussianPDFFast(int state, MathLib::Vector x)
-{
-    double p;
-    gfDiff  = x - gmmpinv[state].MuI;
-    gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
-
-    p = exp(-0.5 * gfDiff.Dot(gfDiffp)) / sqrt(pow(2.0 * PI, nbDimI) * (gmmpinv[state].detSigmaII + 1e-30));
-
-    return p;
-}
-
-double Gaussians::GaussianProbFast(MathLib::Vector x)
-{
-    double totalP = 0;
-    for (unsigned int s = 0; s < model.nbStates; s++)
-    {
-        totalP += model.States[s].Prio * GaussianPDFFast(s, x);
-    }
-    return totalP;
-}
-
-MathLib::Vector Gaussians::GaussianDerProbFast(MathLib::Vector x)
-{
-    gDer.Zero();
-    for (unsigned int s = 0; s < model.nbStates; s++)
-    {
-        gDer += (gmmpinv[s].SigmaIIInv * (x - gmmpinv[s].MuI)) * model.States[s].Prio * GaussianPDFFast(s, x);
-    }
-    return -gDer;
-}
-
-void Gaussians::InitFastGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
-{
-    double det;
-    int nbIN  = last_inindex - first_inindex + 1;
-    int nbOUT = last_outindex - first_outindex + 1;
-
-    gPdf.Resize(model.nbStates);
-
-    for (unsigned int s = 0; s < model.nbStates; s++)
-    {
-        gmmpinv[s].MuI.Resize(nbIN);
-        gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
-        gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
-
-        gmmpinv[s].muO.Resize(nbOUT);
-        gmmpinv[s].SigmaIO.Resize(nbIN, nbOUT);
-        gmmpinv[s].SigmaIOInv.Resize(nbOUT, nbOUT);
-    }
-
-    for (unsigned int s = 0; s < model.nbStates; s++)
-    {
-        for (int i = first_inindex; i <= last_inindex; i++)
-        {
-            gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
-
-            for (int j = first_inindex; j <= last_inindex; j++)
-            {
-                gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
-            }
-            for (int j = first_outindex; j <= last_outindex; j++)
-            {
-                gmmpinv[s].SigmaIO(i - first_inindex, j - first_outindex) = model.States[s].Sigma(i, j);
-            }
-        }
-
-        for (int i = first_outindex; i <= last_outindex; i++)
-        {
-            gmmpinv[s].muO(i - first_outindex) = model.States[s].Mu(i);
-        }
-
-        gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
-        if (det < 0)
-        {
-            det = 1e-30;
-        }
-        gmmpinv[s].detSigmaII = det;
-        (gmmpinv[s].SigmaIO).Transpose().Inverse(gmmpinv[s].SigmaIOInv, &det);
-    }
-
-    nbDimI = last_inindex - first_inindex + 1;
-    gfDiff.Resize(nbDimI);
-    gfDiffp.Resize(nbDimI);
-    gDer.Resize(nbDimI);
-
-}
-
-void Gaussians::Regression(const MathLib::Vector& indata, MathLib::Vector& outdata, MathLib::Matrix& derGMR)
-{
-    Regression(indata, outdata);
-    cout << "derivative is not implemented yet " << endl;
-}
-
-void Gaussians::Regression(const MathLib::Vector& indata, MathLib::Vector& outdata)
-{
-    double pdfall;
-    MathLib::Vector h(model.nbStates);
-    MathLib::Vector r_diff(outdata.Size());
-
-    for (unsigned int s = 0; s < model.nbStates; s++)
-    {
-        gPdf(s) = model.States[s].Prio * GaussianPDFFast(s, indata);
-    }
-    pdfall = gPdf.Sum();
-
-    outdata.Zero();
-    for (unsigned int s = 0; s < model.nbStates; s++)
-    {
-        //h(s) = gPdf(s)/(pdfall + 1e-30 );
-        h(s) = gPdf(s) / (pdfall);
-        r_diff = gmmpinv[s].SigmaIO.Transpose() * gmmpinv[s].SigmaIIInv * (indata - gmmpinv[s].MuI);
-
-        for (unsigned int i = 0; i < r_diff.Size(); i++)
-        {
-            outdata(i) += h(s) * (r_diff(i) + gmmpinv[s].muO(i));
-        }
-    }
-}
-
-MathLib::Vector Gaussians::Regression(const MathLib::Vector& indata)
-{
-    MathLib::Vector outdata(indata.Size());
-    Regression(indata, outdata);
-    return outdata;
-}
-
-
-/*
-#include <math.h>
-#include "Gaussians.h"
-
-#include "armadillo"
-
-using namespace arma;
-using namespace std;
-
-Gaussians::Gaussians(GMMs *model)
-{
-    this->model.nbStates = model->nbStates;
-    this->model.nbDim    = model->nbDim;
-
-    this->model.States = (GMMState  *)malloc(model->nbStates*sizeof(GMMState) );
-
-    for(int s=0; s<model->nbStates; s++ ){
-        this->model.States[s].Mu    = model->GMMState[s].Mu;
-        this->model.States[s].Sigma = model->GMMState[s].Sigma;
-        this->model.States[s].Prio  = model->GMMState[s].Prio;
-    }
-}
-
-Gaussians::Gaussians(int nbStates, int nbDim, char *f_mu, char *f_sigma, char *f_prio)
-{
-
-    int s, i, j;
-
-    model.nbStates = nbStates;
-    model.nbDim    = nbDim;
-    model.States = (GMMState  *)malloc(nbStates*sizeof(GMMState) );
-
-    for( s=0; s<nbStates; s++ ){
-        model.States[s].Mu       =  zeros<vec>(nbDim);
-        model.States[s].Sigma    =  zeros<mat>(nbDim, nbDim );
-    }
-
-    // f_mu
-    ifstream fin;
-    fin.open(f_mu);
-    for( i=0; i<nbDim; i++ ){
-        for( s=0; s<nbStates; s++ ){
-            fin >> model.States[s].Mu(i);
-        }
-    }
-    fin.close();
-
-    // f_sigma
-    fin.open(f_sigma);
-    for( s=0; s<nbStates; s++ ){
-        for( i=0; i<nbDim; i++ ){
-            for( j=0; j<nbDim; j++ ){
-                fin >> model.States[s].Sigma(i,j);
-            }
-        }
-    }
-    fin.close();
-
-    // f_prio
-    fin.open(f_prio);
-    for( s=0; s<nbStates; s++ ){
-        fin >>model.States[s].Prio;
-    }
-    fin.close();
-}
-
-void Gaussians::setGMMs(GMMs *model)
-{
-    for(int s=0; s<model->nbStates; s++ ){
-        this->model.States[s].Mu    = model->GMMState[s].Mu;
-        this->model.States[s].Sigma = model->GMMState[s].Sigma;
-        this->model.States[s].Prio  = model->GMMState[s].Prio;
-    }
-}
-
-
-void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
-{
-    gmmpinv = (GMMStateP *)malloc(model.nbStates*sizeof(GMMStateP) );
-
-    for(int s=0; s<model.nbStates; s++ ){
-        gmmpinv[s].MuI = model.States[s].Mu.subvec(first_inindex, last_inindex);
-        gmmpinv[s].SigmaII = model.States[s].Sigma.submat(first_inindex, first_inindex, last_inindex, last_inindex);
-        gmmpinv[s].SigmaIIInv = pinv(gmmpinv[s].SigmaII);
-        gmmpinv[s].detSigmaII = det(gmmpinv[s].SigmaII);
-    }
-
-    nbDimI = last_inindex - first_inindex +1;
-    gfDiff  = zeros<vec>(nbDimI);
-    gfDiffp = zeros<vec>(nbDimI);
-    gDer    = zeros<vec>(nbDimI);
-}
-
-double Gaussians::GaussianPDFFast(int state, vec x)
-{
-    double p;
-    gfDiff  = x - gmmpinv[state].MuI;
-    gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
-
-    p = exp(-0.5*dot(gfDiff, gfDiffp)) / sqrt(pow(2.0*math::pi(), nbDimI)*( gmmpinv[state].detSigmaII +1e-30));
-
-    return p;
-}
-
-double Gaussians::GaussianProbFast(vec x)
-{
-    double totalP=0;
-    for(int s=0; s<model.nbStates; s++ ){
-        totalP += model.States[s].Prio*GaussianPDFFast(s,x);
-    }
-    return totalP;
-}
-
-vec Gaussians::GaussianDerProbFast(vec x)
-{
-    gDer.zeros();
-    for(int s=0; s<model.nbStates; s++ ){
-        gDer += model.States[s].Prio * gmmpinv[s].SigmaIIInv *(x-gmmpinv[s].MuI)*GaussianPDFFast(s,x);
-    }
-    return -gDer;
-}
-
-//-------------------------------------------------------------------------------------------------------
-void AllocateGMMs(GMMs *model, int nbDim, int nbStates)
-{
-    model->nbDim = nbDim;
-    model->nbStates = nbStates;
-    model->GMMState = (GMMState  *)malloc(nbStates*sizeof(GMMState) );
-
-    for(int s=0; s<nbStates; s++ ){
-        model->GMMState[s].Mu       =  zeros<vec>(nbDim);
-        model->GMMState[s].Sigma    =  zeros<mat>(nbDim, nbDim );
-    }
-}
-
-
-double GaussianPDF(vec x, vec Mu, mat Sigma)
-{
-    double p;
-    vec diff  = x - Mu;
-    vec diffp = pinv( Sigma ) * diff;
-    int nbDim = x.size();
-
-    p = exp(-0.5*dot(diff, diffp)) / sqrt(pow(2.0*math::pi(), nbDim)*( abs(det(Sigma)) +1e-30));
-
-    if(p < 1e-30){
-        return 1e-30;
-    }
-    else{
-        return p;
-    }
-}
-
-void GaussianMux(GMMs *modelK, GMMs *modelL, GMMs *modelOut)
-{
-    int k,l,j;
-    int K = modelK->nbStates;
-    int L = modelL->nbStates;
-    int J = K*L;
-
-    //modelOut->nbDim = modelK->nbDim;
-    //modelOut->nbStates = J;
-    //modelOut->GMMState = (GMMState *)malloc(J*sizeof(GMMState) );
-
-    j=0;
-    for(k=0; k<K; k++){
-        for(l=0; l<L; l++){
-            modelOut->GMMState[j].Sigma = pinv( pinv(modelK->GMMState[k].Sigma) + pinv( modelL->GMMState[l].Sigma) );
-            modelOut->GMMState[j].Mu    = modelOut->GMMState[j].Sigma *( pinv(modelK->GMMState[k].Sigma) * modelK->GMMState[k].Mu + pinv(modelL->GMMState[l].Sigma) * modelL->GMMState[l].Mu );
-            modelOut->GMMState[j].Prio  = modelK->GMMState[k].Prio* modelL->GMMState[l].Prio * GaussianPDF( modelK->GMMState[k].Mu, modelL->GMMState[l].Mu, modelK->GMMState[k].Sigma + modelL->GMMState[l].Sigma);
-            j++;
-        }
-    }
-}
-
-void GaussianRotate(GMMs *model, arma::vec P, arma::mat R, GMMs *modelOut)
-{
-    for(int s=0; s<model->nbStates; s++){
-        modelOut->GMMState[s].Mu    = R*model->GMMState[s].Mu + P;
-        modelOut->GMMState[s].Sigma = R*model->GMMState[s].Sigma*trans(R);
-    }
-    //modelOut->nbDim = model->nbDim;
-    //modelOut->nbStates = model->nbStates;
-}
-*/
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.h b/source/RobotAPI/libraries/DSControllers/Gaussians.h
deleted file mode 100644
index 0e014e8aaf80cf6d413b6ec6ccc0003f7ebecbe7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/Gaussians.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- * Gaussians.h
- *
- *  Created on: Nov 19, 2011
- *      Author: Seungsu KIM
- */
-
-#ifndef __GAUSSIANSM_H__
-#define __GAUSSIANSM_H__
-
-#include "MathLib.h"
-
-#define GAUSSIAN_MAXIMUM_NUMBER 50
-
-struct GMMState
-{
-    MathLib::Vector Mu;
-    MathLib::Matrix Sigma;
-    double Prio;
-};
-
-struct GMMStateP
-{
-    MathLib::Vector MuI;
-    MathLib::Matrix SigmaII;
-    MathLib::Matrix SigmaIIInv;
-    double detSigmaII;
-
-    // for GMR
-    MathLib::Vector muO;
-    MathLib::Matrix SigmaIO;
-    MathLib::Matrix SigmaIOInv;
-};
-
-struct GMMs
-{
-    unsigned int nbStates;
-    unsigned int nbDim;
-
-    GMMState  States[GAUSSIAN_MAXIMUM_NUMBER];
-};
-
-class Gaussians
-{
-private:
-    GMMStateP gmmpinv[GAUSSIAN_MAXIMUM_NUMBER];
-
-public:
-    GMMs model;
-
-    Gaussians(const char* f_mu, const char* f_sigma, const char* f_prio);
-    Gaussians(int nbStates, int nbDim, const char* f_mu, const char* f_sigma, const char* f_prio);
-    Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec);
-    Gaussians(GMMs* model);
-
-    void setGMMs(GMMs* model);
-
-    // For fast computation of GaussianPDF
-    MathLib::Vector gfDiff, gfDiffp;
-    MathLib::Vector gDer;
-    MathLib::Vector gPdf;
-    int nbDimI;
-
-
-    void InitFastGaussians(int first_inindex, int last_inindex);
-    double GaussianPDFFast(int state, MathLib::Vector x);
-    double GaussianProbFast(MathLib::Vector x);
-    MathLib::Vector GaussianDerProbFast(MathLib::Vector x);
-
-    void InitFastGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex);
-    void Regression(const MathLib::Vector& indata, MathLib::Vector& outdata, MathLib::Matrix& derGMR);
-    void Regression(const MathLib::Vector& indata, MathLib::Vector& outdata);
-    MathLib::Vector Regression(const MathLib::Vector& indata);
-
-};
-/*
-void GaussianMux(GMMs *modelK, GMMs *modelL, GMMs *modelOut);
-void GaussianRotate(GMMs *model, Vector P, Matrix R, GMMs *modelOut);
-*/
-
-#endif //__GAUSSIANS_H__
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt b/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
deleted file mode 100644
index ee6932b94c1685bf52c6bf6d37fe9ace33751de7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
+++ /dev/null
@@ -1,87 +0,0 @@
-set(LIB_NAME       KITGripperEtherCAT)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(SOEM QUIET)
-armarx_build_if(SOEM_FOUND "SOEM not available")
-if (SOEM_FOUND)
-    include_directories(SYSTEM ${SOEM_INCLUDE_DIR})
-endif()
-
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-#
-# all include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-#    include_directories(${MyLib_INCLUDE_DIRS})
-#endif()
-
-
-if(ARMARX_USE_QT5)
-    #someone gets qt5::Designer in the link flags!
-    #this is a hotfix for this problem
-    armarx_find_qt(Designer OpenGL)
-    list(APPEND LIBS ${QT_LIBRARIES})
-endif()
-
-
-find_package(Eigen3 QUIET)
-find_package(Simox QUIET)
-
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-
-if (Eigen3_FOUND AND Simox_FOUND)
-    include_directories(${Simox_INCLUDE_DIRS})
-    include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
-endif()
-
-set(LIBS
-	ArmarXCoreInterfaces 
-	ArmarXCore
-        ArmarXEtherCAT
-        ${Simox_LIBRARIES}
-        ${SOEM_LIBRARIES}
-)
-
-set(LIB_FILES
-KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
-KITGripperBasisBoard/KITGripperBasisBoardData.cpp
-KITGripperBasisBoard/KITGripperBasisBoard.cpp
-KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
-KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
-KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
-KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
-KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
-KITGripperBasisBoard/JointController/PWMVelocityController.cpp
-KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
-KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
-KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(LIB_HEADERS
-KITGripperBasisBoard/KITGripperBasisBoardSlave.h
-KITGripperBasisBoard/KITGripperBasisBoardData.h
-KITGripperBasisBoard/KITGripperBasisBoard.h
-KITGripperBasisBoard/Misc/TorqueEstimation.h
-KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
-KITGripperBasisBoard/JointController/JointPWMVelocityController.h
-KITGripperBasisBoard/JointController/JointPWMPositionController.h
-KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h
-KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
-KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
-KITGripperBasisBoard/JointController/PWMVelocityController.h
-KITGripperBasisBoard/JointController/JointZeroTorqueController.h
-KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
-KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
-
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
deleted file mode 100644
index 3bbf673c04a8e6bb1fdaf9d921ad713eeaa65bfa..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-#include "JointKITGripperEmergencyStopController.h"
-
-namespace armarx
-{
-
-    JointKITGripperEmergencyStopController::JointKITGripperEmergencyStopController(ActorDataPtr dataPtr) :
-        dataPtr(dataPtr)
-    {
-        pid.reset(new PIDController(0, 5000, 0));
-        pid->maxIntegral = 0.1;
-    }
-    void JointKITGripperEmergencyStopController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        pid->update(timeSinceLastIteration.toSecondsDouble(), dataPtr->getVelocity(), 0.0);
-        float targetPwm = pid->getControlValue();
-        if (std::isnan(targetPwm))
-        {
-            targetPwm = 0.0f;
-        }
-        //        dataPtr->setTargetPWM(targetPwm);
-        lastPWM *= 0.9997;
-        //        ARMARX_RT_LOGF_INFO("old pwm %d, new pwm: %.2f", dataPtr->getTargetPWM(), lastPWM);
-        dataPtr->setTargetPWM(lastPWM);
-
-    }
-
-    ControlTargetBase* JointKITGripperEmergencyStopController::getControlTarget()
-    {
-        return &target;
-    }
-
-    void JointKITGripperEmergencyStopController::rtPreActivateController()
-    {
-        //        ARMARX_INFO << "Stopping gripper!";
-        //        dataPtr->setTargetPWM(0);
-        lastPWM = math::MathUtils::LimitTo(dataPtr->getTargetPWM(), 500);
-    }
-
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
deleted file mode 100644
index df1c76a4a52997ba35bddfdd15dc7a539b537b20..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-#include "../KITGripperBasisBoardData.h"
-
-
-namespace armarx
-{
-
-    class JointKITGripperEmergencyStopController;
-    typedef std::shared_ptr<JointKITGripperEmergencyStopController> JointKITGripperEmergencyStopControllerPtr;
-
-
-    class JointKITGripperEmergencyStopController : public JointController
-    {
-    public:
-        JointKITGripperEmergencyStopController(ActorDataPtr dataPtr);
-    private:
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-        /**
-         * Returns the Target for this controller, but as this is the Emergency controller it will ignored.
-         * As this controller will just break
-         * @return is type VelocityTarget but it will return a nullptr, because it won't be possible to set a target
-         */
-        ControlTargetBase* getControlTarget() override;
-
-        void rtPreActivateController() override;
-    private:
-        DummyControlTargetEmergencyStop target;
-        ActorDataPtr dataPtr;
-        PIDControllerPtr pid;
-        float lastPWM = 0.0f;
-    };
-
-
-
-} // namespace
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
deleted file mode 100644
index 5e5441eaaf750affbce122d8f65a24fe94a7262f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
+++ /dev/null
@@ -1,29 +0,0 @@
-#include "JointKITGripperPWMPassThroughController.h"
-#include <boost/algorithm/clamp.hpp>
-#include "../KITGripperBasisBoard.h"
-
-namespace armarx
-{
-    ControlTargetBase* JointKITGripperPWMPassThroughController::getControlTarget()
-    {
-        return &target;
-    }
-
-    JointKITGripperPWMPassThroughController::JointKITGripperPWMPassThroughController(const std::string deviceName, ActorDataPtr jointData) :
-        jointData(jointData)
-    {}
-
-    void JointKITGripperPWMPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        if (target.isValid())
-        {
-            jointData->setTargetPWM(target.current * 1000);
-        }
-    }
-
-    void JointKITGripperPWMPassThroughController::rtPreActivateController()
-    {
-        jointData->setTargetPWM(0);
-
-    }
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h
deleted file mode 100644
index 9a367bfec1e2af2d9d4223b0246e2f91d35c734f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include "../KITGripperBasisBoardData.h"
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
-namespace armarx
-{
-
-    class JointKITGripperPWMPassThroughController;
-    typedef std::shared_ptr<JointKITGripperPWMPassThroughController> JointKITGripperPWMPassThroughControllerPtr;
-
-    class JointKITGripperPWMPassThroughController : public armarx::JointController
-    {
-    public:
-        JointKITGripperPWMPassThroughController(const std::string deviceName, ActorDataPtr jointData);
-
-        // JointController interface
-        ControlTargetBase* getControlTarget() override;
-    protected:
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-    private:
-        ControlTarget1DoFActuatorCurrent target;
-        ActorDataPtr jointData;
-
-        // JointController interface
-    protected:
-        void rtPreActivateController() override;
-    };
-
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
deleted file mode 100644
index 6f675f43716e65769b24778d0da8f616aa53da56..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-#include "JointKITGripperStopMovementController.h"
-
-namespace armarx
-{
-    JointKITGripperStopMovementController::JointKITGripperStopMovementController(ActorDataPtr dataPtr) :
-        dataPtr(dataPtr)
-    {
-
-    }
-
-    void JointKITGripperStopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-    }
-
-    ControlTargetBase* JointKITGripperStopMovementController::getControlTarget()
-    {
-        return &target;
-    }
-
-    void JointKITGripperStopMovementController::rtPreActivateController()
-    {
-        //        ARMARX_INFO << "Stopping gripper!";
-        dataPtr->setTargetPWM(0);
-    }
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
deleted file mode 100644
index 1a041018fe1c0feeea13dde44e9e68700c95e2c1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include "../KITGripperBasisBoardData.h"
-
-
-namespace armarx
-{
-
-    class JointKITGripperStopMovementController;
-    typedef std::shared_ptr<JointKITGripperStopMovementController> JointKITGripperStopMovementControllerPtr;
-
-
-    class JointKITGripperStopMovementController : public JointController
-    {
-    public:
-        JointKITGripperStopMovementController(ActorDataPtr dataPtr);
-    private:
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-        /**
-         * Returns the Target for this controller, but as this is the Emergency controller it will ignored.
-         * As this controller will just break
-         * @return is type VelocityTarget but it will return a nullptr, because it won't be possible to set a target
-         */
-        ControlTargetBase* getControlTarget() override;
-
-        void rtPreActivateController() override;
-    private:
-        DummyControlTargetStopMovement target;
-        ActorDataPtr dataPtr;
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
deleted file mode 100644
index d4505ed9937a54904a4aa790b3436c6ab1ab60f5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
+++ /dev/null
@@ -1,327 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "JointPWMPositionController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-#include <boost/algorithm/clamp.hpp>
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
-
-namespace armarx
-{
-    JointPWMPositionController::JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
-            ActorDataPtr jointData,
-            PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) : JointController(),
-        config(positionControllerConfigDataPtr),
-        //    controller(positionControllerConfigDataPtr),
-        target(), board(board), deviceName(deviceName)
-    {
-        actorIndex = board->getActorIndex(deviceName);
-        sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
-        ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
-        dataPtr = jointData;
-
-        posController.desiredDeceleration = positionControllerConfigDataPtr->maxDecelerationRad;
-        posController.desiredJerk = 100;
-        posController.maxDt = positionControllerConfigDataPtr->maxDt;
-        posController.maxV = positionControllerConfigDataPtr->maxVelocityRad;
-        posController.p = 4;
-        posController.phase2SwitchDistance =  0.1;
-        ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
-        //    controller.positionLimitHiHard = dataPtr->getHardLimitHi();
-        //    posController.positionLimitHi = jointData->getSoftLimitHi();
-        //    controller.positionLimitLoHard = dataPtr->getHardLimitLo();
-        //    posController.positionLimitLo = jointData->getSoftLimitLo();
-        //    posController.pControlPosErrorLimit = 0.02f;
-        //    posController.pid->Kp = posController.calculateProportionalGain();
-        //    ARMARX_IMPORTANT << "position Kp " << posController.pid->Kp;
-
-        this->isLimitless = jointData->isLimitless();
-        pidPosController.reset(new PIDController(positionControllerConfigDataPtr->p, positionControllerConfigDataPtr->i, positionControllerConfigDataPtr->d));
-        pidPosController->maxIntegral = positionControllerConfigDataPtr->maxIntegral;
-        pidPosController->differentialFilter.reset(new rtfilters::AverageFilter(10));
-
-    }
-
-    JointPWMPositionController::~JointPWMPositionController() noexcept(true)
-    {
-        stopRequested = true;
-        try
-        {
-            threadHandle.join();
-        }
-        catch (...)
-        {
-
-        }
-    }
-
-    void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        if (target.isValid())
-        {
-            auto currentPosition = dataPtr->getPosition();
-            //        float targetPosition = boost::algorithm::clamp(target.position,
-            //                               std::min(currentPosition, posController.positionLimitLo), // lo or current position
-            //                               std::max(currentPosition, posController.positionLimitHi)); // hi or current position
-
-            if (isLimitless)
-            {
-                ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints").deactivateSpam(10);
-                return;
-
-            }
-            else
-            {
-                //            posController.currentPosition =  currentPosition;
-            }
-            posController.currentV = lastTargetVelocity;
-            posController.dt = timeSinceLastIteration.toSecondsDouble();
-            posController.setTargetPosition(target.position);
-            //        ARMARX_CHECK_EXPRESSION(posController.validParameters());
-            auto r = posController.run();
-            posController.currentPosition = r.position;
-            posController.currentAcc = r.acceleration;
-            double newVel = r.velocity;
-            //        double newVel = posController.p * (posController.targetPosition - posController.currentPosition);
-            //        newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
-            //            ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel);
-            if (std::isnan(newVel))
-            {
-                newVel = 0;
-            }
-            pidPosController->update(timeSinceLastIteration.toSecondsDouble(), currentPosition, r.position);
-            auto targetPWM = pidPosController->getControlValue();
-            //        auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
-            newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
-
-            float torqueFF = config->feedforwardTorqueToPWMFactor * -sensorValue->gravityTorque;
-            targetPWM += torqueFF;
-            targetPWM += newVel * config->feedforwardVelocityToPWMFactor;
-
-            //        ARMARX_INFO << deactivateSpam(0.1) << VAROUT(pidPosController->previousError) << VAROUT(r.acceleration)
-            //                    << VAROUT(target.position) << VAROUT(targetPWM) << VAROUT(pidPosController->Kp) << VAROUT(pidPosController->Ki)
-            //                    << VAROUT(torqueFF);
-
-            if (std::isnan(targetPWM))
-            {
-                ARMARX_ERROR << deactivateSpam(1) << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
-                targetPWM = 0.0f;
-            }
-            float updateRatio = 0.3;
-            this->targetPWM = (1.0 - updateRatio) * this->targetPWM + updateRatio * targetPWM;
-            float pwmDiff = std::abs(dataPtr->getTargetPWM() - targetPWM);
-            //        ARMARX_INFO << deactivateSpam(0.3) << VAROUT(pwmDiff) << VAROUT(dataPtr->getTargetPWM()) << VAROUT(targetPWM) << VAROUT(dataPtr->getVelocity());
-            if (pwmDiff > 5 || dataPtr->getVelocity() > 0.01) // avoid jittering when standing still
-            {
-                //            ARMARX_INFO << deactivateSpam(0.0, std::to_string(targetPWM)) << "Setting new targetPWM to" << targetPWM << " diff: " << pwmDiff << " vel: " << dataPtr->getVelocity();
-                dataPtr->setTargetPWM(targetPWM);
-            }
-
-            this->targetPWM = targetPWM;
-            lastTargetVelocity = newVel;
-            //        auto name = getParent().getDeviceName().c_str();
-            //        ARMARX_RT_LOGF_INFO("%s: position: %.f, target position: %.f, targetvelocity: %.f, target PWM: %d", name,
-            //                            currentPosition, targetPosition, newVel, targetPWM).deactivateSpam(1);
-            //        ARMARX_INFO << deactivateSpam(1) << VAROUT(name) << VAROUT(currentPosition) << VAROUT(targetPosition) << VAROUT(newVel) << VAROUT(targetPWM);
-
-
-        }
-        else
-        {
-            ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
-        }
-    }
-
-    ControlTargetBase* JointPWMPositionController::getControlTarget()
-    {
-        return &target;
-    }
-
-    void JointPWMPositionController::rtPreActivateController()
-    {
-        targetPWM = 0.0f;
-        lastTargetVelocity = dataPtr->getVelocity();
-        posController.currentAcc = dataPtr->getAcceleration();
-        posController.currentPosition = dataPtr->getPosition();
-        posController.currentV = dataPtr->getVelocity();
-        pidPosController->reset();
-        //    controller.reset(dataPtr->getVelocity());
-    }
-
-    void JointPWMPositionController::rtPostDeactivateController()
-    {
-        //    ARMARX_RT_LOGF_INFO("Setting PWM to 0");
-        //    dataPtr->setTargetPWM(0);
-    }
-
-    StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
-    {
-
-        if (!remoteGui)
-        {
-            threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
-            {
-                std::string guiTabName;
-                while (!stopRequested)
-                {
-                    ManagedIceObjectPtr object;
-                    ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
-                    try
-                    {
-                        object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
-                        ARMARX_CHECK_EXPRESSION(object);
-                        remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
-                        if (!remoteGui)
-                        {
-                            continue;
-                        }
-                        ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
-                        guiTabName = getParent().getDeviceName() + getControlMode();
-                        break;
-                    }
-                    catch (...)
-                    {
-                        sleep(1);
-                    }
-
-                }
-                if (remoteGui)
-                {
-                    ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
-                    using namespace RemoteGui;
-
-
-
-                    auto vLayout = makeVBoxLayout();
-
-                    {
-                        WidgetPtr KpLabel = makeTextLabel("Kp: ");
-
-                        WidgetPtr KpSlider = makeFloatSlider("KpSlider")
-                                             .min(0.0f).max(pidPosController->Kp * 5)
-                                             .value(pidPosController->Kp);
-                        WidgetPtr KpLabelValue = makeTextLabel(std::to_string(pidPosController->Kp * 5));
-                        WidgetPtr line = makeHBoxLayout()
-                                         .children({KpLabel, KpSlider, KpLabelValue});
-
-                        vLayout.addChild(line);
-
-                    }
-
-
-                    {
-                        WidgetPtr KiLabel = makeTextLabel("Ki: ");
-                        WidgetPtr KiSlider = makeFloatSlider("KiSlider")
-                                             .min(0.0f).max(pidPosController->Ki * 5)
-                                             .value(pidPosController->Ki);
-                        WidgetPtr KiLabelValue = makeTextLabel(std::to_string(pidPosController->Ki * 5));
-
-                        WidgetPtr line = makeHBoxLayout()
-                                         .children({KiLabel, KiSlider, KiLabelValue});
-
-                        vLayout.addChild(line);
-
-                    }
-
-                    {
-                        WidgetPtr KdLabel = makeTextLabel("Kd: ");
-                        WidgetPtr KdSlider = makeFloatSlider("KdSlider")
-                                             .min(-10.0f * pidPosController->Kd).max(10.0f * pidPosController->Kd)
-                                             .steps(1000)
-                                             .value(pidPosController->Kd);
-                        WidgetPtr KdLabelValue = makeTextLabel(std::to_string(pidPosController->Kd * 10));
-
-                        WidgetPtr line = makeHBoxLayout()
-                                         .children({KdLabel, KdSlider, KdLabelValue});
-
-                        vLayout.addChild(line);
-                        vLayout.addChild(new VSpacer);
-                    }
-
-                    //        WidgetPtr spin = makeFloatSpinBox("KpSpin")
-                    //                         .min(0.0f).max(2.0f)
-                    //                         .steps(20).decimals(2)
-                    //                         .value(0.4f);
-
-
-
-
-                    WidgetPtr groupBox = makeGroupBox("GroupBox")
-                                         .label("Group")
-                                         .child(vLayout);
-
-                    remoteGui->createTab(guiTabName, groupBox);
-
-                    while (!stopRequested)
-                    {
-                        RemoteGui::TabProxy tab(remoteGui, guiTabName);
-                        tab.receiveUpdates();
-                        //                    this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
-                        //                    this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
-                        //                    this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
-                        pidPosController->Kp = tab.getValue<float>("KpSlider").get();
-                        pidPosController->Ki = tab.getValue<float>("KiSlider").get();
-                        pidPosController->Kd = tab.getValue<float>("KdSlider").get();
-                        usleep(100000);
-                    }
-                }
-
-            });
-        }
-        return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
-            {"targetPosition", new Variant(posController.currentPosition)}, // position of profile generator is target position
-            {"posError", new Variant(posController.getTargetPosition() - posController.currentPosition)},
-            {"pidError", new Variant(pidPosController->previousError)},
-            //        {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
-            {"pidIntegralCV", new Variant(pidPosController->integral * pidPosController->Ki)},
-            {"pidIntegral", new Variant(pidPosController->integral)},
-            {"pidPropCV", new Variant(pidPosController->previousError * pidPosController->Kp)},
-            {"pidDiffCV", new Variant(pidPosController->derivative * pidPosController->Kd)},
-            //        {"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
-            //        {"pospidIntegral", new Variant(posController.pid->integral)},
-            //        {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
-            //        {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
-            //        {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
-            {"desiredPWM", new Variant(targetPWM.load())}
-
-
-        };
-    }
-
-
-
-
-
-
-    PWMPositionControllerConfigurationCPtr PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigDataFromXml(DefaultRapidXmlReaderNode node)
-    {
-        PWMPositionControllerConfiguration configData;
-
-        configData.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
-        configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
-        configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
-        configData.maxDt = node.first_node("maxDt").value_as_float();
-        configData.p = node.first_node("p").value_as_float();
-        configData.i = node.first_node("i").value_as_float();
-        configData.d = node.first_node("d").value_as_float();
-        configData.maxIntegral = node.first_node("maxIntegral").value_as_float();
-        configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
-        configData.feedforwardTorqueToPWMFactor = node.first_node("feedforwardTorqueToPWMFactor").value_as_float();
-        configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
-        configData.velocityUpdatePercent = node.first_node("velocityUpdatePercent").value_as_float();
-        configData.conditionalIntegralErrorTreshold = node.first_node("conditionalIntegralErrorTreshold").value_as_float();
-        configData.feedForwardMode = node.first_node("FeedForwardMode").value_as_bool("1", "0");
-
-        return std::make_shared<PWMPositionControllerConfiguration>(configData);
-    }
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
deleted file mode 100644
index 75388a73e0a4f869f3bd76fd9081afc63cb64257..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
+++ /dev/null
@@ -1,88 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
-    using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-    typedef std::shared_ptr<class PWMPositionControllerConfiguration> PWMPositionControllerConfigurationPtr;
-    typedef std::shared_ptr<const PWMPositionControllerConfiguration> PWMPositionControllerConfigurationCPtr;
-
-
-    class PWMPositionControllerConfiguration
-    {
-    public:
-        PWMPositionControllerConfiguration() {}
-        static PWMPositionControllerConfigurationCPtr CreatePWMPositionControllerConfigDataFromXml(DefaultRapidXmlReaderNode node);
-        float maxVelocityRad;
-        float maxAccelerationRad;
-        float maxDecelerationRad;
-        float maxDt;
-        float p;
-        float i;
-        float d;
-        float maxIntegral;
-        float feedforwardVelocityToPWMFactor;
-        float feedforwardTorqueToPWMFactor;
-        float PWMDeadzone;
-        float velocityUpdatePercent;
-        float conditionalIntegralErrorTreshold;
-        bool feedForwardMode;
-    };
-
-
-    class JointPWMPositionController;
-    typedef std::shared_ptr<JointPWMPositionController> JointPWMPositionControllerPtr;
-
-    class JointPWMPositionController : public JointController
-    {
-    public:
-        JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr);
-        ~JointPWMPositionController() noexcept(true);
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
-        ControlTargetBase* getControlTarget() override;
-
-        void rtPreActivateController() override;
-    protected:
-        PWMPositionControllerConfigurationCPtr config;
-        //        PWMVelocityController controller;
-        //        PositionThroughVelocityControllerWithAccelerationAndPositionBounds posController;
-        MinJerkPositionController posController;
-        PIDControllerPtr pidPosController;
-        ControlTarget1DoFActuatorPosition target;
-
-        std::atomic<double> lastTargetVelocity, targetPWM;
-        bool isLimitless;
-
-        ActorDataPtr dataPtr;
-        KITGripperBasisBoardPtr board;
-        const std::string deviceName;
-        size_t actorIndex = 0;
-        mutable RemoteGuiInterfacePrx remoteGui;
-        bool stopRequested = false;
-        mutable ThreadPool::Handle threadHandle;
-        const SensorValue1DoFActuator* sensorValue;
-        // JointController interface
-    protected:
-        void rtPostDeactivateController() override;
-
-        // JointController interface
-    public:
-        StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const override;
-    };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
deleted file mode 100644
index 7451e76c68a41f113af0ed4da9d241ebbc2ca922..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
+++ /dev/null
@@ -1,252 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "JointPWMVelocityController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-namespace armarx
-{
-    JointPWMVelocityController::JointPWMVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
-            PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) : JointController(),
-        config(velocityControllerConfigDataPtr),
-        controller(velocityControllerConfigDataPtr),
-        target(), board(board), deviceName(deviceName)
-    {
-        actorIndex = board->getActorIndex(deviceName);
-        sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
-        ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
-        dataPtr = jointData;
-
-        //    velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
-        velController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
-        velController.jerk = 30;
-        velController.maxDt = velocityControllerConfigDataPtr->maxDt;
-        velController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
-        velController.directSetVLimit = velocityControllerConfigDataPtr->directSetVLimit;
-        ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
-        //    controller.positionLimitHiHard = dataPtr->getHardLimitHi();
-        velController.positionLimitHiSoft = jointData->getSoftLimitHi();
-        //    controller.positionLimitLoHard = dataPtr->getHardLimitLo();
-        velController.positionLimitLoSoft = jointData->getSoftLimitLo();
-        this->isLimitless = jointData->isLimitless();
-    }
-
-    JointPWMVelocityController::~JointPWMVelocityController() noexcept(true)
-    {
-        stopRequested = true;
-        try
-        {
-            threadHandle.join();
-        }
-        catch (...)
-        {
-
-        }
-    }
-
-    void JointPWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        if (target.isValid())
-        {
-
-            {
-                auto currentPosition = dataPtr->getPosition();
-                if (isLimitless)
-                {
-                    velController.currentPosition = velController.positionLimitHiSoft - (velController.positionLimitHiSoft - velController.positionLimitLoSoft) * 0.5;
-                    //                ARMARX_INFO << VAROUT(velController.currentPosition) << VAROUT(velController.positionLimitLoSoft) << VAROUT(velController.positionLimitHiSoft);
-                }
-                else
-                {
-                    velController.currentPosition =  currentPosition;
-                }
-                velController.currentV = lastTargetVelocity;
-                velController.currentAcc = lastTargetAcceleration;
-                velController.dt = timeSinceLastIteration.toSecondsDouble();
-                velController.targetV = target.velocity;
-                auto r = velController.run();
-                double newVel = r.velocity;
-                double newAcc = r.acceleration;
-
-
-                //            ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel) << VAROUT(target.velocity);
-                if (std::isnan(newVel))
-                {
-                    newVel = 0;
-                    newAcc = 0;
-                }
-                //            float newVel = target.velocity;
-                if ((currentPosition > velController.positionLimitHiSoft && target.velocity > 0)
-                    || (currentPosition < velController.positionLimitLoSoft && target.velocity < 0))
-                {
-                    newVel = 0;
-                    newAcc = 0;
-                    ARMARX_INFO << deactivateSpam(1) << "Breaking now at " << dataPtr->getPosition() << " pwm: " << dataPtr->getTargetPWM();
-                }
-
-                auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel, sensorValue->gravityTorque));
-                dataPtr->setTargetPWM(targetPWM);
-
-                lastTargetVelocity = newVel;
-                lastTargetAcceleration = newAcc;
-
-                //            ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
-                //                                target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
-
-            }
-        }
-        else
-        {
-            ARMARX_ERROR << "invalid target set for actor";
-        }
-    }
-
-    ControlTargetBase* JointPWMVelocityController::getControlTarget()
-    {
-        return &target;
-    }
-
-    void JointPWMVelocityController::rtPreActivateController()
-    {
-        lastTargetVelocity = dataPtr->getVelocity();
-        lastTargetAcceleration = dataPtr->getAcceleration();
-        controller.reset(dataPtr->getVelocity());
-    }
-
-    void JointPWMVelocityController::rtPostDeactivateController()
-    {
-        //    ARMARX_RT_LOGF_INFO("Setting PWM to 0");
-        //    dataPtr->setTargetPWM(0);
-    }
-
-    StringVariantBaseMap JointPWMVelocityController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
-    {
-
-        if (!remoteGui && !threadHandle.isValid())
-        {
-            threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
-            {
-                std::string guiTabName;
-                while (!stopRequested)
-                {
-                    ManagedIceObjectPtr object;
-                    ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
-                    try
-                    {
-                        object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
-                        ARMARX_CHECK_EXPRESSION(object);
-                        remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
-                        if (!remoteGui)
-                        {
-                            return;
-                        }
-                        ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
-                        guiTabName = getParent().getDeviceName() + getControlMode();
-                        break;
-                    }
-                    catch (...)
-                    {
-                        handleExceptions();
-                        sleep(1);
-                    }
-
-                }
-                if (remoteGui)
-                {
-                    ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
-                    using namespace RemoteGui;
-
-
-
-                    auto vLayout = makeVBoxLayout();
-
-                    {
-                        WidgetPtr KpLabel = makeTextLabel("Kp: ");
-
-                        WidgetPtr KiSlider = makeFloatSlider("KpSlider")
-                                             .min(0.0f).max(5000.0f)
-                                             .value(config->p);
-                        WidgetPtr line = makeHBoxLayout()
-                                         .children({KpLabel, KiSlider});
-
-                        vLayout.addChild(line);
-
-                    }
-
-
-                    {
-                        WidgetPtr KiLabel = makeTextLabel("Ki: ");
-                        WidgetPtr KiSlider = makeFloatSlider("KiSlider")
-                                             .min(0.0f).max(50000.0f)
-                                             .value(config->i);
-
-                        WidgetPtr line = makeHBoxLayout()
-                                         .children({KiLabel, KiSlider});
-
-                        vLayout.addChild(line);
-
-                    }
-
-                    {
-                        WidgetPtr KdLabel = makeTextLabel("Kd: ");
-                        WidgetPtr KdSlider = makeFloatSlider("KdSlider")
-                                             .min(0.0f).max(50.0f)
-                                             .steps(100)
-                                             .value(config->d);
-
-                        WidgetPtr line = makeHBoxLayout()
-                                         .children({KdLabel, KdSlider});
-
-                        vLayout.addChild(line);
-                        vLayout.addChild(new VSpacer);
-                    }
-
-                    //        WidgetPtr spin = makeFloatSpinBox("KpSpin")
-                    //                         .min(0.0f).max(2.0f)
-                    //                         .steps(20).decimals(2)
-                    //                         .value(0.4f);
-
-
-
-
-                    WidgetPtr groupBox = makeGroupBox("GroupBox")
-                                         .label("Group")
-                                         .child(vLayout);
-
-                    remoteGui->createTab(guiTabName, groupBox);
-
-                    while (!stopRequested)
-                    {
-                        RemoteGui::TabProxy tab(remoteGui, guiTabName);
-                        tab.receiveUpdates();
-                        this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
-                        this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
-                        this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
-                        usleep(100000);
-                    }
-                }
-
-            });
-        }
-        return
-        {
-            {"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
-            {"lastTargetAcceleration", new Variant(lastTargetAcceleration.load())},
-            {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
-            {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
-            {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
-            {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
-
-        };
-    }
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
deleted file mode 100644
index d54e3b7dfa70b014c529c9e4b19b43b6c81e2ba2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
+++ /dev/null
@@ -1,63 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
-    using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-
-
-    class JointPWMVelocityController;
-    typedef std::shared_ptr<JointPWMVelocityController> JointPWMVelocityControllerPtr;
-
-    class JointPWMVelocityController : public JointController
-    {
-    public:
-        JointPWMVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
-        ~JointPWMVelocityController() noexcept(true);
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
-        ControlTargetBase* getControlTarget() override;
-
-        void rtPreActivateController() override;
-    protected:
-        PWMVelocityControllerConfigurationCPtr config;
-        PWMVelocityController controller;
-        VelocityControllerWithRampedAccelerationAndPositionBounds velController;
-
-        ControlTarget1DoFActuatorVelocity target;
-
-        std::atomic<double> lastTargetVelocity, lastTargetAcceleration;
-        bool isLimitless;
-
-        ActorDataPtr dataPtr;
-        KITGripperBasisBoardPtr board;
-        const std::string deviceName;
-        size_t actorIndex = 0;
-        mutable RemoteGuiInterfacePrx remoteGui;
-        bool stopRequested = false;
-        mutable ThreadPool::Handle threadHandle;
-        const SensorValue1DoFActuator* sensorValue;
-        // JointController interface
-    protected:
-        void rtPostDeactivateController() override;
-
-        // JointController interface
-    public:
-        StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const override;
-    };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
deleted file mode 100644
index f6345d0a587d6e75900a828b178f66336a3a51e5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
+++ /dev/null
@@ -1,211 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "JointZeroTorqueController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-namespace armarx
-{
-    PWMZeroTorqueControllerConfigurationCPtr PWMZeroTorqueControllerConfiguration::CreateConfigDataFromXml(DefaultRapidXmlReaderNode node)
-    {
-        PWMZeroTorqueControllerConfiguration configData;
-
-        configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
-        configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
-
-
-        return std::make_shared<PWMZeroTorqueControllerConfiguration>(configData);
-
-    }
-
-    JointPWMZeroTorqueController::JointPWMZeroTorqueController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
-            PWMZeroTorqueControllerConfigurationCPtr config) : JointController(),
-        config(config), target(), board(board), deviceName(deviceName)
-    {
-        actorIndex = board->getActorIndex(deviceName);
-        dataPtr = jointData;
-
-
-        this->isLimitless = jointData->isLimitless();
-
-    }
-
-    JointPWMZeroTorqueController::~JointPWMZeroTorqueController() noexcept(true)
-    {
-        stopRequested = true;
-        try
-        {
-            threadHandle.join();
-        }
-        catch (...)
-        {
-
-        }
-    }
-
-    void JointPWMZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        if (target.isValid())
-        {
-            float targetPWM = dataPtr->getVelocity() * config->feedforwardVelocityToPWMFactor;
-            targetPWM += math::MathUtils::Sign(dataPtr->getVelocity()) * config->PWMDeadzone;
-            //        targetPWM = math::MathUtils::LimitTo(targetPWM, 1500);
-            dataPtr->setTargetPWM(targetPWM);
-
-            //            ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
-            //                                target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
-
-
-        }
-        else
-        {
-            ARMARX_ERROR << "invalid target set for actor";
-        }
-    }
-
-    ControlTargetBase* JointPWMZeroTorqueController::getControlTarget()
-    {
-        return &target;
-    }
-
-    void JointPWMZeroTorqueController::rtPreActivateController()
-    {
-        lastTargetVelocity = dataPtr->getVelocity();
-        //    controller.reset(dataPtr->getVelocity());
-    }
-
-    void JointPWMZeroTorqueController::rtPostDeactivateController()
-    {
-        ARMARX_RT_LOGF_INFO("Setting PWM to 0");
-        dataPtr->setTargetPWM(0);
-    }
-
-    StringVariantBaseMap JointPWMZeroTorqueController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
-    {
-
-        if (!remoteGui && !threadHandle.isValid())
-        {
-            threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
-            {
-                return;
-                //            std::string guiTabName;
-                //            while (!stopRequested)
-                //            {
-                //                ManagedIceObjectPtr object;
-                //                ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
-                //                try
-                //                {
-                //                    object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
-                //                    ARMARX_CHECK_EXPRESSION(object);
-                //                    remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
-                //                    if (!remoteGui)
-                //                    {
-                //                        return;
-                //                    }
-                //                    ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
-                //                    guiTabName = getParent().getDeviceName() + getControlMode();
-                //                    break;
-                //                }
-                //                catch (...)
-                //                {
-                //                    handleExceptions();
-                //                    sleep(1);
-                //                }
-
-                //            }
-                //            if (remoteGui)
-                //            {
-                //                ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
-                //                using namespace RemoteGui;
-
-
-
-                //                //                auto vLayout = makeVBoxLayout();
-
-                //                //                {
-                //                //                    WidgetPtr KpLabel = makeTextLabel("Kp: ");
-
-                //                //                    WidgetPtr KiSlider = makeFloatSlider("KpSlider")
-                //                //                                         .min(0.0f).max(5000.0f)
-                //                //                                         .value(config->p);
-                //                //                    WidgetPtr line = makeHBoxLayout()
-                //                //                                     .children({KpLabel, KiSlider});
-
-                //                //                    vLayout.addChild(line);
-
-                //                //                }
-
-
-                //                //                {
-                //                //                    WidgetPtr KiLabel = makeTextLabel("Ki: ");
-                //                //                    WidgetPtr KiSlider = makeFloatSlider("KiSlider")
-                //                //                                         .min(0.0f).max(50000.0f)
-                //                //                                         .value(config->i);
-
-                //                //                    WidgetPtr line = makeHBoxLayout()
-                //                //                                     .children({KiLabel, KiSlider});
-
-                //                //                    vLayout.addChild(line);
-
-                //                //                }
-
-                //                //                {
-                //                //                    WidgetPtr KdLabel = makeTextLabel("Kd: ");
-                //                //                    WidgetPtr KdSlider = makeFloatSlider("KdSlider")
-                //                //                                         .min(0.0f).max(50.0f)
-                //                //                                         .steps(100)
-                //                //                                         .value(config->d);
-
-                //                //                    WidgetPtr line = makeHBoxLayout()
-                //                //                                     .children({KdLabel, KdSlider});
-
-                //                //                    vLayout.addChild(line);
-                //                //                    vLayout.addChild(new VSpacer);
-                //                //                }
-
-                //                //        WidgetPtr spin = makeFloatSpinBox("KpSpin")
-                //                //                         .min(0.0f).max(2.0f)
-                //                //                         .steps(20).decimals(2)
-                //                //                         .value(0.4f);
-
-
-
-
-                //                WidgetPtr groupBox = makeGroupBox("GroupBox")
-                //                                     .label("Group")
-                //                                     .child(vLayout);
-
-                //                remoteGui->createTab(guiTabName, groupBox);
-
-                //                while (!stopRequested)
-                //                {
-                //                    RemoteGui::TabProxy tab(remoteGui, guiTabName);
-                //                    tab.receiveUpdates();
-                //                    this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
-                //                    this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
-                //                    this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
-                //                    usleep(100000);
-                //                }
-                //            }
-
-            });
-        }
-        return {};
-        //    return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
-        //        {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
-        //        {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
-        //        {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
-        //        {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
-    }
-}
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h
deleted file mode 100644
index 0d71e597184f16c0c7b290b04f6f3821892d5bdd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h
+++ /dev/null
@@ -1,73 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
-    using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-
-
-    typedef std::shared_ptr<class PWMZeroTorqueControllerConfiguration> PWMZeroTorqueControllerConfigurationPtr;
-    typedef std::shared_ptr<const PWMZeroTorqueControllerConfiguration> PWMZeroTorqueControllerConfigurationCPtr;
-
-    class PWMZeroTorqueControllerConfiguration
-    {
-    public:
-        PWMZeroTorqueControllerConfiguration() {}
-        static PWMZeroTorqueControllerConfigurationCPtr CreateConfigDataFromXml(DefaultRapidXmlReaderNode node);
-        float feedforwardVelocityToPWMFactor;
-        float PWMDeadzone;
-    };
-
-
-
-    class JointPWMZeroTorqueController;
-    typedef std::shared_ptr<JointPWMZeroTorqueController> JointPWMZeroTorqueControllerPtr;
-
-    class JointPWMZeroTorqueController : public JointController
-    {
-    public:
-        JointPWMZeroTorqueController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMZeroTorqueControllerConfigurationCPtr config);
-        ~JointPWMZeroTorqueController() noexcept(true);
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
-        ControlTargetBase* getControlTarget() override;
-
-        void rtPreActivateController() override;
-    private:
-        PWMZeroTorqueControllerConfigurationCPtr config;
-        ControlTarget1DoFActuatorZeroTorque target;
-
-        std::atomic<double> lastTargetVelocity;
-        bool isLimitless;
-
-        ActorDataPtr dataPtr;
-        KITGripperBasisBoardPtr board;
-        const std::string deviceName;
-        size_t actorIndex = 0;
-        mutable RemoteGuiInterfacePrx remoteGui;
-        bool stopRequested = false;
-        mutable ThreadPool::Handle threadHandle;
-        // JointController interface
-    protected:
-        void rtPostDeactivateController() override;
-
-        // JointController interface
-    public:
-        StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const override;
-    };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
deleted file mode 100644
index 11911449b906f61c6c5b6263afe049af9f1a1484..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#include "PWMVelocityController.h"
-
-#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
-
-namespace armarx
-{
-
-    PWMVelocityController::PWMVelocityController(PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
-        config(velocityControllerConfigDataPtr)
-    {
-
-
-
-        pid.reset(new PIDController(velocityControllerConfigDataPtr->p,
-                                    velocityControllerConfigDataPtr->i,
-                                    velocityControllerConfigDataPtr->d));
-        pid->pdOutputFilter.reset(new rtfilters::AverageFilter(10));
-        pid->maxIntegral = velocityControllerConfigDataPtr->maxIntegral;
-        pid->conditionalIntegralErrorTreshold = velocityControllerConfigDataPtr->conditionalIntegralErrorTreshold;
-        pid->threadSafe = false;
-    }
-
-    double PWMVelocityController::run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity, double gravityTorque)
-    {
-        double targetPWM = 0;
-        if (!this->config->feedForwardMode)
-        {
-            lastActualVelocity = lastActualVelocity * (1.0 - config->velocityUpdatePercent) + currentVelocity * config->velocityUpdatePercent;
-            pid->update(deltaT.toSecondsDouble(), lastActualVelocity, targetVelocity);
-            targetPWM = pid->getControlValue();
-        }
-        float torqueFF = config->feedforwardTorqueToPWMFactor * -gravityTorque;
-        //        ARMARX_INFO << deactivateSpam(1) << VAROUT(torqueFF);
-        targetPWM += torqueFF;
-
-
-        //feed forward
-        if (std::abs(targetVelocity) > 0.001 && std::abs(currentVelocity) < 0.0001f)
-        {
-            targetPWM += config->PWMDeadzone * math::MathUtils::Sign(targetVelocity); // deadzone
-        }
-        targetPWM += config->feedforwardVelocityToPWMFactor * targetVelocity; // approx. feedforward vel
-
-
-
-
-        //            ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
-        //                                target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
-
-
-
-        return targetPWM;
-    }
-
-    void PWMVelocityController::reset(double currentVelocity)
-    {
-        lastActualVelocity = currentVelocity;
-        pid->reset();
-    }
-
-    PWMVelocityControllerConfigurationCPtr PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigDataFromXml(DefaultRapidXmlReaderNode node)
-    {
-        PWMVelocityControllerConfiguration configData;
-
-        configData.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
-        configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
-        configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
-        configData.maxDt = node.first_node("maxDt").value_as_float();
-        configData.directSetVLimit = node.first_node("directSetVLimit").value_as_float();
-        configData.p = node.first_node("p").value_as_float();
-        configData.i = node.first_node("i").value_as_float();
-        configData.d = node.first_node("d").value_as_float();
-        configData.maxIntegral = node.first_node("maxIntegral").value_as_float();
-        configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
-        configData.feedforwardTorqueToPWMFactor = node.first_node("feedforwardTorqueToPWMFactor").value_as_float();
-        configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
-        configData.velocityUpdatePercent = node.first_node("velocityUpdatePercent").value_as_float();
-        configData.conditionalIntegralErrorTreshold = node.first_node("conditionalIntegralErrorTreshold").value_as_float();
-        configData.feedForwardMode = node.first_node("FeedForwardMode").value_as_bool("1", "0");
-
-        return std::make_shared<PWMVelocityControllerConfiguration>(configData);
-
-    }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
deleted file mode 100644
index 622060bbe7a056b353c7b818a6fd41fbd667e55f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-#include <atomic>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-
-#include "PWMVelocityController.h"
-
-namespace armarx
-{
-
-    typedef std::shared_ptr<class PWMVelocityControllerConfiguration> PWMVelocityControllerConfigurationPtr;
-    typedef std::shared_ptr<const PWMVelocityControllerConfiguration> PWMVelocityControllerConfigurationCPtr;
-
-    class PWMVelocityControllerConfiguration
-    {
-    public:
-        PWMVelocityControllerConfiguration() {}
-        static PWMVelocityControllerConfigurationCPtr CreatePWMVelocityControllerConfigDataFromXml(DefaultRapidXmlReaderNode node);
-        float maxVelocityRad;
-        float maxAccelerationRad;
-        float maxDecelerationRad;
-        float maxDt;
-        float directSetVLimit;
-        float p;
-        float i;
-        float d;
-        float maxIntegral;
-        float feedforwardVelocityToPWMFactor;
-        float feedforwardTorqueToPWMFactor;
-        float PWMDeadzone;
-        float velocityUpdatePercent;
-        float conditionalIntegralErrorTreshold;
-        bool feedForwardMode;
-    };
-
-    class PWMVelocityController
-    {
-    public:
-        PWMVelocityController(PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
-        double run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity, double gravityTorque);
-        void reset(double currentVelocity);
-
-        PWMVelocityControllerConfigurationCPtr config;
-
-        PIDControllerPtr pid;
-        std::atomic<double> lastActualVelocity;
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
deleted file mode 100644
index f2f558638188351c8efb27a8c403d43956063d8a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ParallelGripperPositionController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-namespace armarx
-{
-    ParallelGripperPositionController::ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
-            ActorDataPtr jointData,
-            PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) :
-        JointPWMPositionController(deviceName, board, jointData, positionControllerConfigDataPtr)
-    {
-        linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
-    }
-
-    ParallelGripperPositionController::~ParallelGripperPositionController() noexcept(true)
-    {
-
-    }
-
-    void ParallelGripperPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        if (target.isValid())
-        {
-            float linkedPositionFactor = 2.0 / 3.0;
-            target.position += (linkedDataPtr->getRelativePosition() * linkedPositionFactor);
-            ARMARX_RT_LOGF_INFO("target.position %.2f, relative partner pos: %.2f", target.position, linkedDataPtr->getRelativePosition()).deactivateSpam(0.5);
-            JointPWMPositionController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
-        }
-        else
-        {
-            ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
-        }
-    }
-
-    void ParallelGripperPositionController::rtPreActivateController()
-    {
-        linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
-        ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
-        JointPWMPositionController::rtPreActivateController();
-    }
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
deleted file mode 100644
index b645f126bc7c0e6815bcdf0c461dde2efdb70f16..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "JointPWMPositionController.h"
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
-    using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-
-
-    class ParallelGripperPositionController;
-    typedef std::shared_ptr<ParallelGripperPositionController> ParallelGripperPositionControllerPtr;
-
-    class ParallelGripperPositionController : public JointPWMPositionController
-    {
-    public:
-        ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr);
-        ~ParallelGripperPositionController() noexcept(true);
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
-    private:
-        uint32_t linkedJointConnectorIndex = -1;
-        ActorDataPtr linkedDataPtr;
-
-
-
-        // JointController interface
-    protected:
-        void rtPreActivateController() override;
-    };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
deleted file mode 100644
index 3ed5f029b283a0e34b161e4b996dea2aa3f4dfa2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ParallelGripperVelocityController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-namespace armarx
-{
-    ParallelGripperVelocityController::ParallelGripperVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
-            PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
-        JointPWMVelocityController(deviceName, board, jointData, velocityControllerConfigDataPtr)
-    {
-        this->linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
-
-    }
-
-    ParallelGripperVelocityController::~ParallelGripperVelocityController() noexcept(true)
-    {
-
-    }
-
-    void ParallelGripperVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        if (target.isValid())
-        {
-            float linkedVelocityFactor = 2.0f / 3.0f;
-            target.velocity += linkedVelocityFactor * linkedDataPtr->getVelocity();
-            JointPWMVelocityController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
-        }
-        else
-        {
-            ARMARX_ERROR << "invalid target set for actor";
-        }
-    }
-
-
-    void ParallelGripperVelocityController::rtPreActivateController()
-    {
-        linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
-        ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
-        JointPWMVelocityController::rtPreActivateController();
-    }
-}
-
-
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
deleted file mode 100644
index 9ea5eca89df0ed1c5c53b849857af129a791995e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "JointPWMVelocityController.h"
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
-    using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-
-
-    class ParallelGripperVelocityController;
-    typedef std::shared_ptr<ParallelGripperVelocityController> ParallelGripperVelocityControllerPtr;
-
-    class ParallelGripperVelocityController : public JointPWMVelocityController
-    {
-    public:
-        ParallelGripperVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
-        ~ParallelGripperVelocityController() noexcept(true);
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
-
-        void rtPreActivateController() override;
-    private:
-
-        uint32_t linkedJointConnectorIndex = -1;
-
-        ActorDataPtr linkedDataPtr;
-
-    };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
deleted file mode 100644
index 378a6111c59af4f7c87488736fbcaf26d1f5afe9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
+++ /dev/null
@@ -1,297 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#include "KITGripperBasisBoard.h"
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "JointController/JointPWMPositionController.h"
-#include "JointController/JointZeroTorqueController.h"
-#include "JointController/JointKITGripperEmergencyStopController.h"
-#include "JointController/JointKITGripperStopMovementController.h"
-#include "JointController/JointPWMVelocityController.h"
-#include "JointController/JointKITGripperPWMPassThroughController.h"
-#include "JointController/ParallelGripperPositionController.h"
-#include "JointController/ParallelGripperVelocityController.h"
-#include "Misc/TorqueEstimation.h"
-namespace armarx
-{
-    VirtualDeviceFactory::SubClassRegistry KITGripperBasisBoard::registry("KITGripperBasisBoard", &VirtualDeviceFactory::createInstance<KITGripperBasisBoard>);
-
-    KITGripperBasisBoard::KITGripperBasisBoard(RapidXmlReaderNode node, DefaultRapidXmlReaderNode defaultConfigurationNode, const VirtualRobot::RobotPtr& robot) :
-        DeviceBase(node.attribute_value("name")),
-        SensorDevice(node.attribute_value("name")),
-        //        ControlDevice(node.attribute_value("name")),
-        AbstractFunctionalDevice(defaultConfigurationNode.first_node("KITGripperBasisBoardDefaultConfiguration")
-                                 .add_node_at_end(node)),
-        configNode(node),
-        defaultConfigurationNode(defaultConfigurationNode),
-        robot(robot),
-        slaveIdentifier(node.first_node("Identifier"))
-    {
-        slaveIdentifier.humanName = node.attribute_value("name");
-        ARMARX_VERBOSE << "found " << configNode.nodes("Actor").size() << " actors";
-        for (auto motorNode : configNode.nodes("Actor"))
-        {
-            auto connectorIndex = motorNode.attribute_as_uint("connector");
-            auto name = motorNode.attribute_value("name");
-            auto enabled = motorNode.attribute_as_bool("enabled", "true", "false");
-
-            if (enabled)
-            {
-                ARMARX_VERBOSE << "Found motor configuration for connector index " << connectorIndex;
-                //                auto actorData = dataPtr->getActorData(connectorIndex);
-                //                ARMARX_CHECK_EXPRESSION_W_HINT(actorData, name);
-                auto robotNode = robot->getRobotNode(name);
-                ARMARX_CHECK_EXPRESSION_W_HINT(robotNode, name);
-                ARMARX_INFO << "Creating actor class for " << name;
-                KITGripperBasisBoard::ActorRobotUnitDevicePtr ptr  = std::make_shared<KITGripperBasisBoard::ActorRobotUnitDevicePtr::element_type>(connectorIndex, name, robotNode);
-                devices.push_back(ptr);
-
-            }
-            else
-            {
-                ARMARX_INFO << "motor at Index " << connectorIndex << " disabled";
-            }
-        }
-    }
-
-    void KITGripperBasisBoard::init(KITGripperBasisBoardSlavePtr slave)
-    {
-        this->slave = slave;
-        initialized = true;
-
-    }
-
-    void KITGripperBasisBoard::initData()
-    {
-        dataPtr = std::make_shared<KITGripperBasisBoardData>(configNode, defaultConfigurationNode,
-                  slave->getOutputsPtr(), slave->getInputsPtr(), robot);
-
-        for (auto motorNode : configNode.nodes("Actor"))
-        {
-            auto enabled = motorNode.attribute_as_bool("enabled", "true", "false");
-            auto name = motorNode.attribute_value("name");
-            if (enabled)
-            {
-                auto i = getActorIndex(name);
-                devices.at(i)->init(std::dynamic_pointer_cast<KITGripperBasisBoard>(shared_from_this()), dataPtr,
-                                    motorNode, defaultConfigurationNode);
-            }
-
-        }
-
-    }
-
-    const SensorValueBase* KITGripperBasisBoard::getSensorValue() const
-    {
-        return &sensorValue;
-    }
-
-    const SlaveIdentifier& KITGripperBasisBoard::getSlaveIdentifier() const
-    {
-        return slaveIdentifier;
-    }
-
-    void KITGripperBasisBoard::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        dataPtr->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
-
-        // TODO: read IMU
-        sensorValue.IMUTemperature = dataPtr->getIMUTemperature();
-    }
-
-    //    void KITGripperBasisBoard::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    //    {
-    //        // TODO: write LED targets
-    //    }
-
-    const std::vector<KITGripperBasisBoard::ActorRobotUnitDevicePtr>& KITGripperBasisBoard::getDevices() const
-    {
-        return devices;
-    }
-
-    size_t KITGripperBasisBoard::getActorIndex(const std::string& actorName)
-    {
-        size_t i = 0;
-        for (auto& actor : devices)
-        {
-            if (actor->getRobotNode()->getName() == actorName)
-            {
-                return i;
-            }
-            i++;
-        }
-        throw LocalException() << "Could not find actor with name: " << actorName << "\nactors:\n" << ARMARX_STREAM_PRINTER { for (auto& actor : devices)
-    {
-        out << actor->getDeviceName();
-        }
-                                                                                                                            };
-    }
-
-    KITGripperBasisBoard::ActorRobotUnitDevice::ActorRobotUnitDevice(size_t connectorIndex, const std::string& deviceName, VirtualRobot::RobotNodePtr robotNode) :
-        DeviceBase(deviceName),
-        ControlDevice(deviceName),
-        SensorDevice(deviceName),
-        actorIndex(connectorIndex)
-    {
-        ARMARX_CHECK_EXPRESSION_W_HINT(robotNode, deviceName);
-        this->robotNode = robotNode;
-        ARMARX_INFO << deviceName << " actor created";
-    }
-
-    void KITGripperBasisBoard::ActorRobotUnitDevice::init(KITGripperBasisBoardPtr dev, KITGripperBasisBoardDataPtr dataPtr,
-            RapidXmlReaderNode configNode, DefaultRapidXmlReaderNode defaultConfigurationNode)
-    {
-        this->board = dev;
-        this->actorDataPtr = dataPtr->getActorData(actorIndex);
-        emergencyController.reset(new JointKITGripperEmergencyStopController(actorDataPtr));
-        addJointController(emergencyController.get());
-        stopMovementController.reset(new JointKITGripperStopMovementController(actorDataPtr));
-        addJointController(stopMovementController.get());
-        auto positionControllerCfg =   PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigDataFromXml(
-                                           defaultConfigurationNode.first_node("JointPWMPositionControllerDefaultConfiguration")
-                                           .add_node_at_end(configNode.first_node("JointPWMPositionControllerConfig")));
-
-        auto velocityControllerCfg =   PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigDataFromXml(
-                                           defaultConfigurationNode.first_node("JointPWMVelocityControllerDefaultConfiguration")
-                                           .add_node_at_end(configNode.first_node("JointPWMVelocityControllerConfig")));
-        auto zeroTorqueControllerCfg =   PWMZeroTorqueControllerConfiguration::CreateConfigDataFromXml(
-                                             defaultConfigurationNode.first_node("JointPWMZeroTorqueControllerDefaultConfiguration")
-                                             .add_node_at_end(configNode.first_node("JointPWMZeroTorqueControllerConfig")));
-
-        pwmController.reset(new JointKITGripperPWMPassThroughController(getDeviceName(), actorDataPtr));
-        addJointController(pwmController.get());
-        //        ARMARX_CHECK_EQUAL_W_HINT(
-        //            configNode.has_node("ParallelGripperDecoupplingFactor"),
-        //            configNode.has_node("SiblingConnectorId"),
-        //            "Either both or none have to be set.");
-        //        auto tempConfigNode = defaultConfigurationNode.first_node("KITGripperActorDefaultConfiguration").
-        //                              add_node_at_end(configNode);
-        //        parallelGripperDecouplingFactor = tempConfigNode.first_node("ParallelGripperDecoupplingFactor").value_as_float();
-
-        //        if (configNode.has_node("ParallelGripperDecoupplingFactor"))
-        //        {
-        //            const int siblingConnectorId = configNode.first_node("SiblingConnectorId").value_as_int32();
-
-        //            //get sibling
-        //            for (const ActorRobotUnitDevicePtr& dev : board->devices)
-        //            {
-        //                if (dev->actorIndex == siblingConnectorId)
-        //                {
-        //                    sibling = dev.get();
-        //                    break;
-        //                }
-        //            }
-        //            ARMARX_CHECK_NOT_NULL_W_HINT(sibling, "Sibling with connector index " << siblingConnectorId << " not found");
-        //            ARMARX_INFO << "Device " << board->getDeviceName() << ", actor " << getDeviceName() << " is using "
-        //                        << sibling->getDeviceName() << " as a sibling for relative position sensor values";
-        //        }
-
-        if (false && actorDataPtr->getSiblingControlActorIndex() != -1)
-        {
-            ARMARX_IMPORTANT << "Using coupled mode for " << getDeviceName();
-            // Add Controllers for ParallelGripper
-            if (actorDataPtr->getVelocityControlEnabled())
-            {
-                velocityController.reset(new ParallelGripperVelocityController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
-                addJointController(velocityController.get());
-            }
-            else
-            {
-                ARMARX_VERBOSE << "Velocity Control disabled for " << getDeviceName();
-            }
-            if (actorDataPtr->getPositionControlEnabled())
-            {
-                positionController.reset(new ParallelGripperPositionController(getDeviceName(), dev, actorDataPtr, positionControllerCfg));
-                addJointController(positionController.get());
-            }
-            else
-            {
-                ARMARX_VERBOSE << "Position Control disabled for " << getDeviceName();
-            }
-            //            //TODO: Does PG get zero torque controller? If so, a special one?
-            //            zeroTorqueController.reset(new JointPWMZeroTorqueController(getDeviceName(), dev, actorPtr, zeroTorqueControllerCfg));
-            //            addJointController(zeroTorqueController.get());
-            return;
-        }
-        else
-        {
-            if (actorDataPtr->getVelocityControlEnabled())
-            {
-                velocityController.reset(new JointPWMVelocityController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
-                addJointController(velocityController.get());
-            }
-            else
-            {
-                ARMARX_VERBOSE << "Velocity Control disabled for " << getDeviceName();
-            }
-            zeroTorqueController.reset(new JointPWMZeroTorqueController(getDeviceName(), dev, actorDataPtr, zeroTorqueControllerCfg));
-            addJointController(zeroTorqueController.get());
-            if (actorDataPtr->getPositionControlEnabled())
-            {
-                positionController.reset(new JointPWMPositionController(getDeviceName(), dev, actorDataPtr, positionControllerCfg));
-                addJointController(positionController.get());
-            }
-            else
-            {
-                ARMARX_VERBOSE << "Position Control disabled for " << getDeviceName();
-            }
-        }
-    }
-
-    ActorDataPtr KITGripperBasisBoard::ActorRobotUnitDevice::getActorDataPtr() const
-    {
-        return actorDataPtr;
-    }
-
-    const VirtualRobot::RobotNodePtr& KITGripperBasisBoard::ActorRobotUnitDevice::getRobotNode() const
-    {
-        return robotNode;
-    }
-
-    void KITGripperBasisBoard::ActorRobotUnitDevice::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        sensorValue.position = actorDataPtr->getPosition();
-        sensorValue.relativePosition = actorDataPtr->getRelativePosition();
-        sensorValue.velocity = actorDataPtr->getVelocity();
-        sensorValue.acceleration = actorDataPtr->getAcceleration();
-        sensorValue.absoluteEncoderVelocity = actorDataPtr->getAbsoluteEncoderVelocity();
-        sensorValue.targetPWM = actorDataPtr->getTargetPWM();
-        sensorValue.motorCurrent = actorDataPtr->getTargetPWM();
-        sensorValue.minPWM = actorDataPtr->getCurrentMinPWM();
-        sensorValue.maxPWM = actorDataPtr->getCurrentMaxPWM();
-        sensorValue.velocityTicksPerMs = actorDataPtr->getVelocityTicks();
-        sensorValue.torque = estimateTorque(sensorValue.velocityTicksPerMs, sensorValue.targetPWM);
-    }
-
-    void KITGripperBasisBoard::ActorRobotUnitDevice::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-
-
-    }
-
-} // namespace
-
-
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
deleted file mode 100644
index 6164bc90aa8814d33a0fa42650553f201a8de6b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
+++ /dev/null
@@ -1,139 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include "KITGripperBasisBoardData.h"
-#include "KITGripperBasisBoardSlave.h"
-
-#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
-#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
-
-#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h>
-
-namespace armarx
-{
-    using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-    using JointKITGripperStopMovementControllerPtr = std::shared_ptr<class JointKITGripperStopMovementController>;
-    using JointKITGripperEmergencyStopControllerPtr = std::shared_ptr<class JointKITGripperEmergencyStopController>;
-    using JointPWMVelocityControllerPtr = std::shared_ptr<class JointPWMVelocityController>;
-    using JointPWMPositionControllerPtr = std::shared_ptr<class JointPWMPositionController>;
-    using JointPWMZeroTorqueControllerPtr = std::shared_ptr<class JointPWMZeroTorqueController>;
-    using JointKITGripperPWMPassThroughControllerPtr = std::shared_ptr<class JointKITGripperPWMPassThroughController>;
-    using ParallelGripperPositionControllerPtr = std::shared_ptr<class ParallelGripperPositionController>;
-    using ParallelGripperVelocityControllerPtr = std::shared_ptr<class ParallelGripperVelocityController>;
-    using PWMVelocityControllerConfigurationPtr = std::shared_ptr<class PWMVelocityControllerConfiguration>;
-
-
-    class KITGripperBasisBoard :
-        public SensorDevice,
-    //        public ControlDevice,
-        public AbstractFunctionalDevice
-    {
-        static VirtualDeviceFactory::SubClassRegistry registry;
-    public:
-
-        class ActorRobotUnitDevice :
-            public ControlDevice,
-            public SensorDevice
-        {
-            friend class KITGripperBasisBoard;
-
-            // SensorDevice interface
-        public:
-            ActorRobotUnitDevice(size_t connectorIndex, const std::string& deviceName, VirtualRobot::RobotNodePtr robotNode);
-            const SensorValueBase* getSensorValue() const override
-            {
-                return &sensorValue;
-            }
-
-            void init(KITGripperBasisBoardPtr dev, KITGripperBasisBoardDataPtr actorDataPtr, RapidXmlReaderNode configNode, DefaultRapidXmlReaderNode defaultConfigurationNode);
-        protected:
-            KITGripperBasisBoardPtr board;
-            //            KITGripperBasisBoardDataPtr dataPtr;
-            VirtualRobot::RobotNodePtr robotNode;
-            size_t actorIndex;
-            ActorDataPtr actorDataPtr;
-
-            JointKITGripperEmergencyStopControllerPtr emergencyController;
-            JointKITGripperStopMovementControllerPtr stopMovementController;
-            JointPWMVelocityControllerPtr velocityController;
-            JointPWMPositionControllerPtr positionController;
-            JointPWMZeroTorqueControllerPtr zeroTorqueController;
-            JointKITGripperPWMPassThroughControllerPtr pwmController;
-
-            /// The data object for copying to non-rt part
-            KITGripperActorSensorData sensorValue;
-
-
-            // SensorDevice interface
-        public:
-            void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-            // ControlDevice interface
-        public:
-            void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-            const VirtualRobot::RobotNodePtr& getRobotNode() const;
-            ActorDataPtr getActorDataPtr() const;
-        };
-        using ActorRobotUnitDevicePtr = std::shared_ptr<ActorRobotUnitDevice>;
-
-        KITGripperBasisBoard(RapidXmlReaderNode node, DefaultRapidXmlReaderNode defaultConfigurationNode, VirtualRobot::RobotPtr const& robot);
-        void init(KITGripperBasisBoardSlavePtr slave);
-
-        // AbstractFunctionalDevice interface
-    public:
-        void initData() override;
-
-        // SensorDevice interface
-    public:
-        const SensorValueBase* getSensorValue() const override;
-        const SlaveIdentifier& getSlaveIdentifier() const;
-        const std::vector<ActorRobotUnitDevicePtr >& getDevices() const;
-        size_t getActorIndex(const std::string& actorName);
-    protected:
-        // SensorDevice interface
-    public:
-        void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        //        // ControlDevice interface
-        //    public:
-        //        void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-    private:
-        RapidXmlReaderNode configNode;
-        DefaultRapidXmlReaderNode defaultConfigurationNode;
-        VirtualRobot::RobotPtr robot;
-        KITGripperBasisBoardDataPtr dataPtr;
-        KITGripperBasisBoardSlavePtr slave;
-        SensorValueKITGripperBasisBoard sensorValue;
-        SlaveIdentifier slaveIdentifier;
-        std::vector<ActorRobotUnitDevicePtr > devices;
-        PWMVelocityControllerConfigurationPtr velocityControllerConfigDataPtr;
-
-    };
-
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
deleted file mode 100644
index a7b49f24a8d539eef51ae7a0076f4cee330a0c12..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+++ /dev/null
@@ -1,324 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#include "KITGripperBasisBoardData.h"
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-namespace armarx
-{
-
-    KITGripperBasisBoardData::KITGripperBasisBoardData(const RapidXmlReaderNode& node, DefaultRapidXmlReaderNode defaultConfigurationNode, KITGripperBasisBoardOUT_t* sensorOUT, KITGripperBasisBoardIN_t* sensorIN, VirtualRobot::RobotPtr robot) :
-        sensorOUT(sensorOUT),
-        sensorIN(sensorIN)
-    {
-        ARMARX_CHECK_EXPRESSION(sensorOUT);
-        ARMARX_CHECK_EXPRESSION(sensorIN);
-        actorData.resize(3);
-        for (auto motorNode : node.nodes("Actor"))
-        {
-            auto connectorIndex = motorNode.attribute_as_uint("connector");
-            auto name = motorNode.attribute_value("name");
-            //auto enabled = motorNode.attribute_as_bool("enabled", "true", "false");
-            auto conversionNode = defaultConfigurationNode.first_node("KITGripperBasisBoardConversionParametersDefaultConfig").
-                                  add_node_at_end(motorNode.first_node("ConversionParameters"));
-            auto configNode = defaultConfigurationNode.first_node("KITGripperActorDefaultConfiguration").
-                              add_node_at_end(motorNode);
-            auto positionControlEnabled = configNode.first_node("PositionControlEnabled").value_as_bool("1", "0");
-            auto velocityControlEnabled = configNode.first_node("VelocityControlEnabled").value_as_bool("1", "0");
-            ARMARX_IMPORTANT << "Creating actor data class for " << name << " at index " << connectorIndex;
-            auto initActorData = [&](int* position, int* velocity, int* torque, int* targetPWM)
-            {
-                actorData.at(connectorIndex).reset(new ActorData);
-                actorData.at(connectorIndex)->targetPWMPtr.init(targetPWM, conversionNode.first_node("pwm"), std::nan("1"), true, "targetPWMPtr");
-                actorData.at(connectorIndex)->maxPWM = configNode.first_node("maxPWM").value_as_uint32();
-                actorData.at(connectorIndex)->position.init(&actorData.at(connectorIndex)->rawABSEncoderTicks, conversionNode.first_node("position"), std::nan("1"), true, "position");
-                actorData.at(connectorIndex)->relativePosition.init(position, conversionNode.first_node("relativePosition"), std::nan("1"), true, "relativePosition");
-                actorData.at(connectorIndex)->velocity.init(velocity, conversionNode.first_node("velocity"), std::nan("1"), true, "velocity");
-                actorData.at(connectorIndex)->torque.init(torque, conversionNode.first_node("torque"), std::nan("1"), true, "torque");
-                actorData.at(connectorIndex)->robotNode = robot->getRobotNode(name);
-                actorData.at(connectorIndex)->velocityTicks = velocity;
-                actorData.at(connectorIndex)->positionControlEnabled = positionControlEnabled;
-                actorData.at(connectorIndex)->velocityControlEnabled = velocityControlEnabled;
-
-                actorData.at(connectorIndex)->parallelGripperDecouplingFactor = configNode.first_node("ParallelGripperDecouplingFactor").value_as_float();
-                actorData.at(connectorIndex)->parallelControlEnabled = configNode.first_node("ParallelControlSiblingIndex").value_as_int32();
-                actorData.at(connectorIndex)->currentPWMBoundGradient = configNode.first_node("CurrentPWMBoundGradient").value_as_float();
-                actorData.at(connectorIndex)->currentPWMBoundOffset = configNode.first_node("CurrentPWMBoundOffset").value_as_int32();
-            };
-            switch (connectorIndex)
-            {
-                case 0:
-                    initActorData(&sensorOUT->motor1_current_pos, &sensorOUT->motor1_current_speed, &sensorOUT->motor1_current_torque, &sensorIN->motor1_target_pwm);
-                    break;
-                case 1:
-                    initActorData(&sensorOUT->motor2_current_pos, &sensorOUT->motor2_current_speed, &sensorOUT->motor2_current_torque, &sensorIN->motor2_target_pwm);
-                    break;
-                case 2:
-                    initActorData(&sensorOUT->motor3_current_pos, &sensorOUT->motor3_current_speed, &sensorOUT->motor3_current_torque, &sensorIN->motor3_target_pwm);
-                    break;
-                default:
-                    throw LocalException("Motor index out of range: ") << connectorIndex;
-            }
-
-        }
-    }
-
-    void KITGripperBasisBoardData::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        int i = 0;
-        double dt = timeSinceLastIteration.toSecondsDouble();
-        for (auto& ptr : actorData)
-        {
-            if (!ptr)
-            {
-                ++i;
-                continue;
-            }
-            ActorData& d = *ptr;
-
-
-            d.relativePosition.read();
-
-            if (d.getSiblingControlActorIndex() >= 0)
-            {
-                auto& d2 = actorData.at(d.getSiblingControlActorIndex());
-                d.adjustedRelativePosition = d.relativePosition.value +
-                                             d2->relativePosition.value * d.parallelGripperDecouplingFactor;
-            }
-            else
-            {
-                d.adjustedRelativePosition = d.relativePosition.value;
-            }
-
-            if (std::isnan(d.relativePositionOffset)) // initialize on first run
-            {
-                d.relativePositionOffset = -d.relativePosition.value + d.position.value;
-            }
-
-            if (i == 0)
-            {
-                d.rawABSEncoderTicks = (((uint32_t)sensorOUT->RawABSEncoderValueBytes[0] << 24 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[1] << 16 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[2] << 8 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[3]) & 0xFFFFF000) >> 12;
-            }
-            else if (i == 1)
-            {
-                d.rawABSEncoderTicks = (((uint32_t)sensorOUT->RawABSEncoder2ValueBytes[0] << 24 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[1] << 16 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[2] << 8 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[3]) & 0xFFFFF000) >> 12;
-            }
-            else
-            {
-                d.rawABSEncoderTicks = 0;
-            }
-
-
-            if (i > 1)
-            {
-                d.position.value = d.adjustedRelativePosition;
-            }
-            else
-            {
-                d.position.read();
-            }
-            d.sanitizedAbsolutePosition = math::MathUtils::angleModPI(d.position.value);
-
-            //ARMARX_RT_LOGF_INFO("position %d, relative position: %d", (int)d.rawABSEncoderTicks, d.relativePosition.value).deactivateSpam(0.5);
-            if (!std::isnan(d.lastAbsolutePosition))
-            {
-                d.absoluteEncoderVelocity = math::MathUtils::AngleDelta(d.lastAbsolutePosition, d.sanitizedAbsolutePosition) / timeSinceLastIteration.toSecondsDouble();
-            }
-            d.lastAbsolutePosition = d.sanitizedAbsolutePosition;
-            float oldVelocity = d.velocity.value;
-            d.velocity.read();
-            d.velocity.value = d.velocityFilter.update(sensorValuesTimestamp, d.velocity.value);
-            d.torque.read();
-            d.currentMaxPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient + d.currentPWMBoundOffset);
-            d.currentMinPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient - d.currentPWMBoundOffset);
-            d.acceleration = (d.velocity.value - oldVelocity) / dt;
-            //            d.acceleration.read();
-            //            d.gravityTorque.read();
-            //            d.motorCurrent.read();
-            //            d.motorTemperature.read();
-            i++;
-        }
-
-    }
-
-    void KITGripperBasisBoardData::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        //        int i = 0;
-        //        for (auto& ptr : actorData)
-        //        {
-        //            if (!ptr)
-        //            {
-        //                ++i;
-        //                continue;
-        //            }
-        //            ActorData& d = *ptr;
-        //            d.targetPWMPtr.write();
-
-        //            i++;
-        //        }
-
-        for (auto& ptr : actorData)
-        {
-            if (!ptr)
-            {
-
-                continue;
-            }
-            ActorData& d = *ptr;
-            if (this->getIMUTemperature() > 87)
-            {
-                ARMARX_RT_LOGF_WARNING("IMU Temperature of a gripper board is too high! %d degree celcius").deactivateSpam(5);
-                d.setTargetPWM(0);
-            }
-        }
-    }
-
-    ActorDataPtr& KITGripperBasisBoardData::getActorData(size_t actorIndex)
-    {
-        ARMARX_CHECK_LESS(actorIndex, actorData.size());
-        ARMARX_CHECK_EXPRESSION_W_HINT(actorData.at(actorIndex), actorIndex);
-        return actorData.at(actorIndex);
-    }
-
-    int8_t KITGripperBasisBoardData::getIMUTemperature() const
-    {
-        return sensorOUT->IMUTemperature;
-    }
-
-    ActorData::ActorData() :
-        velocityFilter(10)
-    {
-
-    }
-
-    void ActorData::setTargetPWM(int32_t targetPWM)
-    {
-        targetPWM = math::MathUtils::LimitMinMax(currentMinPWM, currentMaxPWM, targetPWM);
-        targetPWM = math::MathUtils::LimitTo(targetPWM, maxPWM);
-
-        targetPWMPtr.value = targetPWM;
-        targetPWMPtr.write();
-        //        ARMARX_RT_LOGF_INFO(" pwm: %d, raw pwm: %d, factor: %.f", targetPWMPtr.value, targetPWMPtr.getRaw(), targetPWMPtr.getFactor());
-    }
-
-    float ActorData::getPosition() const
-    {
-        return sanitizedAbsolutePosition;
-    }
-
-    float ActorData::getRelativePosition() const
-    {
-        return adjustedRelativePosition;
-    }
-
-    float ActorData::getCompensatedRelativePosition() const
-    {
-        return adjustedRelativePosition + relativePositionOffset;
-    }
-
-    float ActorData::getVelocity() const
-    {
-        return velocity.value;
-    }
-
-    float ActorData::getAcceleration() const
-    {
-        return acceleration;
-    }
-
-    float ActorData::getTorque() const
-    {
-        return torque.value;
-    }
-
-    float ActorData::getSoftLimitHi() const
-    {
-        return robotNode->getJointLimitHigh();
-    }
-
-    float ActorData::getSoftLimitLo() const
-    {
-        return robotNode->getJointLimitLo();
-    }
-
-    bool ActorData::isLimitless() const
-    {
-        return robotNode->isLimitless();
-    }
-
-    int32_t ActorData::getTargetPWM() const
-    {
-        return targetPWMPtr.value;
-    }
-
-    int32_t ActorData::getVelocityTicks() const
-    {
-        return *velocityTicks;
-    }
-
-    int32_t ActorData::getCurrentMinPWM() const
-    {
-        return currentMinPWM;
-    }
-
-    int32_t ActorData::getCurrentMaxPWM() const
-    {
-        return currentMaxPWM;
-    }
-
-    int32_t ActorData::getSiblingControlActorIndex() const
-    {
-        return parallelControlEnabled;
-    }
-
-    bool ActorData::getPositionControlEnabled() const
-    {
-        return positionControlEnabled;
-    }
-
-    bool ActorData::getVelocityControlEnabled() const
-    {
-        return velocityControlEnabled;
-    }
-
-    float ActorData::getCurrentPWMBoundGradient() const
-    {
-        return currentPWMBoundGradient;
-    }
-
-    int32_t ActorData::getCurrentPWMBoundOffset() const
-    {
-        return currentPWMBoundOffset;
-    }
-
-    float ActorData::getParallelGripperDecouplingFactor() const
-    {
-        return parallelGripperDecouplingFactor;
-    }
-
-    float ActorData::getAbsoluteEncoderVelocity() const
-    {
-        return absoluteEncoderVelocity;
-    }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
deleted file mode 100644
index d5952c687fdca3f0e03a9ad437f46bf04333495c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
+++ /dev/null
@@ -1,183 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
-#include "KITGripperBasisBoardSlave.h"
-
-namespace armarx
-{
-    class   SensorValueKITGripperBasisBoard :
-    //            virtual public SensorValue1DoFActuatorMotorTemperature,
-        virtual public SensorValueIMU
-    {
-    public:
-        DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-
-        int IMUTemperature;
-
-
-        static SensorValueInfo<SensorValueKITGripperBasisBoard> GetClassMemberInfo()
-        {
-            SensorValueInfo<SensorValueKITGripperBasisBoard> svi;
-            //            svi.addBaseClass<SensorValue1DoFActuatorMotorTemperature>();
-            svi.addMemberVariable(&SensorValueKITGripperBasisBoard::IMUTemperature, "IMUTemperature");
-            svi.addBaseClass<SensorValueIMU>();
-            return svi;
-        }
-        SensorValueKITGripperBasisBoard() = default;
-        SensorValueKITGripperBasisBoard(SensorValueKITGripperBasisBoard&&) = default;
-        SensorValueKITGripperBasisBoard(const SensorValueKITGripperBasisBoard&) = default;
-        SensorValueKITGripperBasisBoard& operator=(SensorValueKITGripperBasisBoard&& other)
-        {
-            *this = other;
-            return *this;
-        }
-        SensorValueKITGripperBasisBoard& operator=(const SensorValueKITGripperBasisBoard&) = default;
-
-    };
-
-    class KITGripperActorSensorData : virtual public SensorValue1DoFRealActuator
-    {
-    public:
-        DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-        int32_t targetPWM;
-        float relativePosition;
-        float velocityTicksPerMs;
-        float absoluteEncoderVelocity;
-        int32_t maxPWM;
-        int32_t minPWM;
-
-        static SensorValueInfo<KITGripperActorSensorData> GetClassMemberInfo()
-        {
-            SensorValueInfo<KITGripperActorSensorData> svi;
-            //            svi.addBaseClass<SensorValue1DoFActuatorMotorTemperature>();
-            svi.addBaseClass<SensorValue1DoFRealActuator>();
-            svi.addMemberVariable(&KITGripperActorSensorData::targetPWM, "targetPWM");
-            svi.addMemberVariable(&KITGripperActorSensorData::relativePosition, "relativePosition");
-            svi.addMemberVariable(&KITGripperActorSensorData::maxPWM, "maxPWM");
-            svi.addMemberVariable(&KITGripperActorSensorData::minPWM, "minPWM");
-            svi.addMemberVariable(&KITGripperActorSensorData::velocityTicksPerMs, "velocityTicksPerMs");
-            svi.addMemberVariable(&KITGripperActorSensorData::absoluteEncoderVelocity, "absoluteEncoderVelocity");
-
-            return svi;
-        }
-
-    };
-
-
-    class ActorData
-    {
-    public:
-        ActorData();
-        void setTargetPWM(int32_t targetPWM);
-        float getPosition() const;
-        float getRelativePosition() const;
-        float getCompensatedRelativePosition() const;
-        float getVelocity() const;
-        float getAcceleration() const;
-        float getTorque() const;
-        float getSoftLimitHi() const;
-        float getSoftLimitLo() const;
-        bool isLimitless() const;
-        int32_t getTargetPWM() const;
-        int32_t getVelocityTicks() const;
-
-        int32_t getCurrentMinPWM() const;
-
-        int32_t getCurrentMaxPWM() const;
-
-        int32_t getSiblingControlActorIndex() const;
-
-        bool getPositionControlEnabled() const;
-
-        bool getVelocityControlEnabled() const;
-
-        float getCurrentPWMBoundGradient() const;
-
-        int32_t getCurrentPWMBoundOffset() const;
-
-        float getParallelGripperDecouplingFactor() const;
-
-        float getAbsoluteEncoderVelocity() const;
-
-    private:
-        u_int32_t rawABSEncoderTicks;
-        LinearConvertedValue<int32_t> relativePosition;
-        float adjustedRelativePosition;
-        float relativePositionOffset = std::nan("");
-        float parallelGripperDecouplingFactor = std::nanf("");
-        LinearConvertedValue<u_int32_t> position;
-        float sanitizedAbsolutePosition;
-        LinearConvertedValue<int32_t> velocity;
-        rtfilters::AverageFilter velocityFilter;
-        float absoluteEncoderVelocity = 0.0f;
-        float acceleration;
-        float lastAbsolutePosition = std::nanf("");
-        LinearConvertedValue<int32_t> torque;
-        LinearConvertedValue<int32_t> targetPWM;
-        int32_t* velocityTicks;
-        int32_t currentMaxPWM = 0;
-        int32_t currentMinPWM = 0;
-        size_t maxPWM;
-        int32_t parallelControlEnabled = -1;
-        VirtualRobot::RobotNodePtr robotNode;
-        LinearConvertedValue<int32_t> targetPWMPtr;
-        bool positionControlEnabled = true;
-        bool velocityControlEnabled = true;
-        float currentPWMBoundGradient = 3.75;
-        int32_t currentPWMBoundOffset = 1500;
-        friend class KITGripperBasisBoardData;
-    };
-    using ActorDataPtr = std::shared_ptr<ActorData>;
-
-    class KITGripperBasisBoardData : public AbstractData
-    {
-    public:
-        KITGripperBasisBoardData(const RapidXmlReaderNode& node, DefaultRapidXmlReaderNode defaultConfigurationNode,
-                                 KITGripperBasisBoardOUT_t* sensorOUT, KITGripperBasisBoardIN_t* sensorIN, VirtualRobot::RobotPtr robot);
-
-        // AbstractData interface
-    public:
-        void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-        void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-        ActorDataPtr& getActorData(size_t actorIndex);
-
-        int8_t getIMUTemperature() const;
-
-    private:
-        KITGripperBasisBoardOUT_t* sensorOUT;
-        KITGripperBasisBoardIN_t* sensorIN;
-
-        std::vector<ActorDataPtr> actorData;
-
-    };
-    using KITGripperBasisBoardDataPtr = std::shared_ptr<KITGripperBasisBoardData>;
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
deleted file mode 100644
index fa31a5a3c225a0356f9ee3852584e4b1bf551c8d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#include "KITGripperBasisBoard.h"
-#include "KITGripperBasisBoardSlave.h"
-
-#include <ethercat.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h>
-
-namespace armarx
-{
-
-
-
-    KITGripperBasisBoardSlave::KITGripperBasisBoardSlave(const SlaveIdentifier slaveIdentifier, uint16_t slaveNumber) :
-        AbstractSlaveWithInputOutput(slaveIdentifier, slaveNumber)
-    {
-        setTag("KITGripperBasisBoardSlave_" + slaveIdentifier.humanName);
-    }
-
-    void KITGripperBasisBoardSlave::doMappings()
-    {
-    }
-
-    bool KITGripperBasisBoardSlave::prepare()
-    {
-        return true;
-    }
-
-    void KITGripperBasisBoardSlave::execute()
-    {
-        /*if (::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance())
-        {
-            //ARMARX_RT_LOGF_INFO("relative position 1: %d, current speed 1: %d", (int)outputs->motor1_current_pos, (int)outputs->motor1_current_speed).deactivateSpam(0.5);
-            //ARMARX_RT_LOGF_INFO("relative position 2: %d, current speed 2: %d", (int)outputs->motor2_current_pos, (int)outputs->motor2_current_speed).deactivateSpam(0.5);
-            //ARMARX_RT_LOGF_INFO("relative position 3: %d, current speed 3: %d", (int)outputs->motor3_current_pos, (int)outputs->motor3_current_speed).deactivateSpam(0.5);
-        }*/
-    }
-
-    bool KITGripperBasisBoardSlave::shutdown()
-    {
-        return true;
-    }
-
-    void KITGripperBasisBoardSlave::prepareForOp()
-    {
-    }
-
-    bool KITGripperBasisBoardSlave::hasError()
-    {
-        return false;
-    }
-
-    /**
-     * register this class in the super class factory
-     */
-    KITGripperBasisBoardFactory::SubClassRegistry KITGripperBasisBoardFactory::registry(KITGripperBasisBoardFactory::getName(), &KITGripperBasisBoardFactory::createInstance);
-
-    KITGripperBasisBoardFactory::SharedPointerType KITGripperBasisBoardFactory::createInstance(EtherCATFactoryArgs args)
-    {
-        EtherCAT* etherCAT = std::get<0>(args);
-        ARMARX_CHECK_EXPRESSION(etherCAT);
-        auto slaveIndex = std::get<1>(args);
-        auto deviceContainer = std::get<2>(args);
-
-        auto devs = deviceContainer->getDevicesOfType<KITGripperBasisBoard>();
-        //        if ((ec_slave[slaveIndex].mbx_proto & ECT_MBXPROT_COE) == 0) // TODO: valid for this slave?
-        {
-            for (auto& dev : devs)
-            {
-                if (ec_slave[slaveIndex].eep_man == H2T_VENDOR_ID &&
-                    ec_slave[slaveIndex].eep_id == dev->getSlaveIdentifier().ProductID)
-                {
-                    ARMARX_INFO << "KITGripperBasisBoard '" << dev->getSlaveIdentifier().humanName << "' found";
-                    auto slave = std::make_shared<KITGripperBasisBoardSlave>(dev->getSlaveIdentifier(), slaveIndex);
-                    dev->init(slave);
-                    KITGripperBasisBoardFactoryPtr objFac(new KITGripperBasisBoardFactory);
-                    objFac->addSlave(slave);
-                    objFac->addSensorDevice(dev);
-                    for (auto& virtualDev : dev->getDevices())
-                    {
-                        objFac->addControlDevice(virtualDev);
-                        objFac->addSensorDevice(virtualDev);
-                    }
-                    return objFac;
-                }
-
-            }
-        }
-        return SharedPointerType();
-    }
-
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
deleted file mode 100644
index 0a38422a6ebbcafb78e301276435dfe220dcbbf3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ArmarX
- * @author     Mirko Waechter( mirko.waechter at kit dot edu)
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h>
-#include <memory>
-
-namespace armarx
-{
-
-    /**
-     * @brief PDO mapping sensorB->master
-     */
-    struct KITGripperBasisBoardOUT_t
-    {
-        u_int8_t RawABSEncoderValueBytes[4];
-        u_int8_t RawABSEncoderValueCRC;
-        int16_t pad1;
-        int8_t pad2;
-
-        u_int8_t RawABSEncoder2ValueBytes[4];
-        u_int8_t RawABSEncoder2ValueCRC;
-        int16_t pad3;
-        int8_t pad4;
-
-        int16_t IMUVector1[3];
-        int16_t IMUVector2[3];
-        int16_t IMUQuaternionW;
-        int16_t IMUQuaternionX;
-        int16_t IMUQuaternionY;
-        int16_t IMUQuaternionZ;
-        int8_t IMUTemperature;
-
-        int16_t pad5;
-        int8_t pad6;
-
-
-        int32_t motor1_current_pos;
-        int32_t motor1_current_speed; // ticks pro milliseconds
-        int32_t motor1_current_torque;
-        int32_t motor2_current_pos;
-        int32_t motor2_current_speed;
-        int32_t motor2_current_torque;
-        int32_t motor3_current_pos;
-        int32_t motor3_current_speed;
-        int32_t motor3_current_torque;
-
-    } __attribute__((__packed__));
-
-    /**
-     * @brief PDO mapping master->sensorB
-     */
-    struct KITGripperBasisBoardIN_t
-    {
-        uint16_t LED_PG15;
-        uint16_t LED_2;
-        uint16_t LED_3;
-        uint16_t LED_4;
-
-        int32_t motor1_target_pwm;
-        int32_t motor1_target_speed;
-        int32_t motor1_target_torque;
-        int32_t motor2_target_pwm;
-        int32_t motor2_target_speed;
-        int32_t motor2_target_torque;
-        int32_t motor3_target_pwm;
-        int32_t motor3_target_speed;
-        int32_t motor3_target_torque;
-        int32_t motor4_target_pwm;
-    } __attribute__((__packed__));
-
-    class KITGripperBasisBoardSlave;
-    typedef std::shared_ptr<KITGripperBasisBoardSlave> KITGripperBasisBoardSlavePtr;
-
-
-    class KITGripperBasisBoardSlave :
-        public AbstractSlaveWithInputOutput<KITGripperBasisBoardIN_t, KITGripperBasisBoardOUT_t>
-    {
-    public:
-        KITGripperBasisBoardSlave(const armarx::SlaveIdentifier slaveIdentifier, uint16_t slaveNumber);
-
-        // AbstractSlave interface
-    public:
-        void doMappings() override;
-        bool prepare() override;
-        void execute() override;
-        bool shutdown() override;
-        void prepareForOp() override;
-        bool hasError() override;
-    };
-
-    class KITGripperBasisBoardFactory : public EtherCATDeviceFactory
-    {
-        KITGripperBasisBoardFactory() {}
-        // AbstractFactoryMethod
-    public:
-        static std::string getName()
-        {
-            return "KITGripperBasisBoardFactory";
-        }
-    private:
-        static SubClassRegistry registry;
-        static SharedPointerType createInstance(EtherCATFactoryArgs args);
-    };
-    using KITGripperBasisBoardFactoryPtr = std::shared_ptr<KITGripperBasisBoardFactory>;
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
deleted file mode 100644
index 07b083078de1f8acbbaddc6352dfc43e747a83e4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
+++ /dev/null
@@ -1,261 +0,0 @@
-#pragma once
-/* Includes ------------------------------------------------------------------*/
-
-#include "TorqueEstimationWeights.h"
-
-float imgIn[5];
-float imgFcl1[32];
-float imgFcl2[32];
-float imgFcl3[32];
-float imgFcl4[16];
-float imgFcl5[8];
-float imgFcl6[1];
-
-
-uint8_t fcl1(void)
-{
-    uint8_t outFNum = sizeof imgFcl1 / sizeof(float);
-    uint8_t inFNum = sizeof imgIn / sizeof(float);
-    uint8_t outF = 0;
-    uint8_t inF = 0;
-    float buf = 0.0;
-    memset(imgFcl1, 0.0, sizeof(imgFcl1));
-
-    for (outF = 0; outF < outFNum; outF++)
-    {
-        for (inF = 0; inF < inFNum; inF++)
-        {
-            buf += imgIn[inF] * fc1Weights[inF][outF]; //OLDENTRIES-1-
-        }
-
-        buf += fc1Bias[outF];
-        if (buf < 0)    //relu
-        {
-            buf = 0.0;
-        }
-        imgFcl1[outF] = buf;
-        buf = 0.;
-    }
-    return 1;
-}
-
-
-uint8_t fcl2(void)
-{
-    uint8_t outFNum = sizeof imgFcl2 / sizeof(float);
-    uint8_t inFNum = sizeof imgFcl1 / sizeof(float);
-    uint8_t outF = 0;
-    uint8_t inF = 0;
-    float buf = 0.0;
-    memset(imgFcl2, 0.0, sizeof(imgFcl2));
-
-    for (outF = 0; outF < outFNum; outF++)
-    {
-        for (inF = 0; inF < inFNum; inF++)
-        {
-            buf += imgFcl1[inF] * fc2Weights[inF][outF];
-        }
-
-        buf += fc2Bias[outF];
-        if (buf < 0)    //relu
-        {
-            buf = 0.0;
-        }
-        imgFcl2[outF] = buf;
-        buf = 0.;
-    }
-    return 1;
-}
-
-uint8_t fcl3(void)
-{
-    uint8_t outFNum = sizeof imgFcl3 / sizeof(float);
-    uint8_t inFNum = sizeof imgFcl2 / sizeof(float);
-    uint8_t outF = 0;
-    uint8_t inF = 0;
-    float buf = 0.0;
-    memset(imgFcl3, 0.0, sizeof(imgFcl3));
-
-    for (outF = 0; outF < outFNum; outF++)
-    {
-        for (inF = 0; inF < inFNum; inF++)
-        {
-            buf += imgFcl2[inF] * fc3Weights[inF][outF];
-        }
-
-        buf += fc3Bias[outF];
-        if (buf < 0)    //relu
-        {
-            buf = 0.0;
-        }
-        imgFcl3[outF] = buf;
-        buf = 0.;
-    }
-    return 1;
-}
-uint8_t fcl4(void)
-{
-    uint8_t outFNum = sizeof imgFcl4 / sizeof(float);
-    uint8_t inFNum = sizeof imgFcl3 / sizeof(float);
-    uint8_t outF = 0;
-    uint8_t inF = 0;
-    float buf = 0.0;
-    memset(imgFcl4, 0.0, sizeof(imgFcl4));
-
-    for (outF = 0; outF < outFNum; outF++)
-    {
-        for (inF = 0; inF < inFNum; inF++)
-        {
-            buf += imgFcl3[inF] * fc4Weights[inF][outF];
-        }
-
-        buf += fc4Bias[outF];
-        if (buf < 0)    //relu
-        {
-            buf = 0.0;
-        }
-        imgFcl4[outF] = buf;
-        buf = 0.;
-    }
-    return 1;
-}
-
-uint8_t fcl5(void)
-{
-    uint8_t outFNum = sizeof imgFcl5 / sizeof(float);
-    uint8_t inFNum = sizeof imgFcl4 / sizeof(float);
-    uint8_t outF = 0;
-    uint8_t inF = 0;
-    float buf = 0.0;
-    memset(imgFcl5, 0.0, sizeof(imgFcl5));
-
-    for (outF = 0; outF < outFNum; outF++)
-    {
-        for (inF = 0; inF < inFNum; inF++)
-        {
-            buf += imgFcl4[inF] * fc5Weights[inF][outF];
-        }
-
-        buf += fc5Bias[outF];
-        if (buf < 0)    //relu
-        {
-            buf = 0.0;
-        }
-        imgFcl5[outF] = buf;
-        buf = 0.;
-    }
-    return 1;
-}
-
-uint8_t fcl6(void)
-{
-    uint8_t outFNum = sizeof imgFcl6 / sizeof(float);
-    uint8_t inFNum = sizeof imgFcl5 / sizeof(float);
-    uint8_t outF = 0;
-    uint8_t inF = 0;
-    float buf = 0.0;
-    memset(imgFcl6, 0.0, sizeof(imgFcl6));
-
-    for (outF = 0; outF < outFNum; outF++)
-    {
-        for (inF = 0; inF < inFNum; inF++)
-        {
-            buf += imgFcl5[inF] * fc6Weights[inF][outF];
-        }
-
-        buf += fc6Bias[outF];
-        //if(buf < 0)   //relu
-        //{
-        //  buf = 0.0;
-        //}
-        imgFcl6[outF] = buf;
-        buf = 0.;
-    }
-    return 1;
-}
-
-
-float linearModel_dcx22(int32_t nI, int32_t pwm)
-{
-    float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> U/min
-    float T_f = 1000. / 231.; //übersetzung getriebe + Nm->m
-    float motor_a = 226. + 30.; //Drehzahlkonstante   [min-1 V-1]
-    float motor_b = 123.;   //Kennliniensteigung [min-1 mNm-1]
-    float motor_eta = 0.4; //Wirkungsgrad Getriebe *Motor, max from datasheet:0.75*0.85
-    float Umax = 48.; //Spannung bei pwm max
-    float pwmmax = 3000.;
-    float pwm_zero = 250.;
-    float U;
-    float T_motor;
-
-
-    U = (float(fabs(pwm) - pwm_zero) / (pwmmax - pwm_zero))  *   Umax;
-    if (U < 0)
-    {
-        U = 0;
-    }
-    if (pwm < 0)
-    {
-        U *= -1;
-    }
-    if (pwm == 0)
-    {
-        U = 0;
-    }
-
-    //U(M,n)=(n-b*M)/a
-    T_motor = (U * motor_a - n) / -motor_b;
-    auto T = T_motor * motor_eta / T_f;
-
-    return T;
-}
-
-
-float estimateTorque(int32_t n, int32_t pwm)
-{
-    float n_input = (float)n / n_factor;
-    float pwm_input = (float)pwm / pwm_factor;
-    //    float inputData[6];
-    static float pwmXn_old = 0;
-    float pwmXn = n_input * pwm_input;
-    float torque = 0.;
-
-    if (pwmXn < 0)
-    {
-        pwmXn = -1;
-        pwmXn_old = pwmXn;
-    }
-    else if (pwmXn > 0)
-    {
-        pwmXn = 1;
-        pwmXn_old = pwmXn;
-    }
-    else
-    {
-        pwmXn = pwmXn_old;
-    }
-
-    //    powerDir = linearModel_dcx22(n, pwm);
-    //    if(powerDir < 0)
-    //    {
-    //            powerDir = -1;
-    //    }
-    //    if(powerDir > 0 )
-    //    {
-    //            powerDir = 1;
-    //    }
-
-    imgIn[0] = n_input;
-    imgIn[1] = pwm_input;
-    imgIn[2] = n_input;
-    imgIn[3] = pwm_input;
-    imgIn[4] = pwmXn;
-    fcl1();
-    fcl2();
-    fcl3();
-    fcl4();
-    fcl5();
-    fcl6();
-    torque = imgFcl6[0];
-    return torque + linearModel_dcx22(n, pwm);
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
deleted file mode 100644
index 2ef7dfd6cb02eb3de517e327b2b393995ff92b8e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
+++ /dev/null
@@ -1,189 +0,0 @@
-// written from class_nn_T_prediction_11_Delta_simple2.py;   testName:aktor_l
-//2018-12-20 15:15
-#ifndef __WEIGHTS_H
-#define __WEIGHTS_H
-
-static float n_factor = 459.0;
-static float pwm_factor = 1700.0;
-
-
-const float fc1Weights[5][32] =
-{
-    {-0.2055357695f, 0.1997956634f, 0.0573679060f, 5.6070003510f, -0.0359823219f, -0.6203082204f, -3.5134258270f, -0.0713983923f, 0.0685816705f, -0.0381882004f, -1.7217775583f, -3.8536903858f, -0.3756493032f, -0.1594707817f, -6.3813128471f, -0.3226724863f, 0.2063246369f, -0.6387553811f, 0.0372939110f, 3.8864912987f, 1.4991726875f, 4.3318767548f, -1.9102532864f, -0.0856776312f, -0.5104419589f, 0.3844818473f, -0.5385549664f, 0.0184886176f, -0.2233840525f, 0.3443737030f, -0.1556139588f, 8.5595130920f},
-    {0.2853892446f, 0.9347148538f, 0.2580135465f, -5.0483689308f, -0.1692553312f, 0.6664142609f, -0.8050979972f, -0.1831562817f, 0.2196862400f, 0.0588629805f, 2.8238334656f, 4.0935006142f, 0.3776662350f, 0.1199822798f, 6.6613926888f, -0.1866912246f, 0.0059743905f, -4.2951517105f, -0.1723812819f, -3.6761276722f, 1.8258478642f, -1.4123811722f, 1.8817381859f, -0.3389021456f, 0.4059905112f, -0.3896758854f, 0.7206512094f, 0.2583603859f, -2.8177876472f, -0.2914963961f, 0.4590151906f, -9.9321269989f},
-    {-0.5632920861f, 0.1988572478f, -0.5542681813f, 5.7822799683f, 0.3820048273f, -0.2525918782f, -3.3970746994f, 0.2208536267f, 0.0045266999f, 0.3900515437f, -1.7668331861f, -3.6709997654f, -0.2005964518f, 0.1454627365f, -6.3249039650f, 0.2582260370f, -0.2433532774f, -0.9390440583f, -0.5737978816f, 4.1182165146f, 1.6590707302f, 4.8758029938f, -1.8478810787f, 0.0048876349f, 0.1774665117f, -0.1789420843f, -0.5673617721f, -0.2723075449f, -0.3745986819f, 0.0240579937f, -0.2824915946f, 8.3010101318f},
-    {0.4449025393f, 0.6115619540f, 0.0187926833f, -5.4022917747f, 0.0334641673f, 0.4216177166f, -0.8479185700f, -0.4422490299f, -0.2795608342f, -0.3466927111f, 2.2280035019f, 3.5217893124f, 0.1864205599f, -0.1767526865f, 6.5718340874f, 0.3395559490f, -0.0630193949f, -4.2917470932f, 0.4928318858f, -3.4380824566f, 2.2824761868f, -1.3895356655f, 1.8980522156f, 0.3309353590f, 0.0092654871f, 0.2050092816f, 0.4185230136f, -0.0931341946f, -3.0571875572f, -0.0199668277f, 0.0238261241f, -10.0575485229f},
-    {-0.0596951246f, -1.8140755892f, -0.1749392897f, 0.2104561627f, -0.2132787555f, -0.1574463993f, -6.4865732193f, -0.7267509699f, -0.1174281165f, -0.0856533423f, 0.4403370619f, -0.6915963888f, -0.2986042798f, -0.3891125619f, 1.3234760761f, 0.0340486430f, -0.0694809407f, -7.1216664314f, -0.1790750027f, 1.6705595255f, -5.9638838768f, -0.9045498967f, -0.1985210180f, -0.1773930788f, -0.1488362402f, -0.0660701320f, -0.0532661825f, -0.1185604334f, 0.3091884255f, -0.1427213848f, -0.0880882740f, 1.5660281181f}
-};
-
-
-const float fc1Bias[32] =
-{-0.2996839881f, -2.2546701431f, -0.5477045178f, -1.2242631912f, -0.2746987045f, -0.5186939240f, -6.1057929993f, -0.9974002242f, -0.2025506645f, -0.2643158436f, -0.5174910426f, -0.6349564195f, -0.5265557766f, -0.5465639234f, -0.8746579289f, -0.2214775383f, -0.3936641812f, -9.3305616379f, -0.3976268172f, -0.0949812457f, -4.9294414520f, -0.2506873310f, -1.1473840475f, -0.2789034545f, -0.2843038142f, -0.1568380445f, -0.3574894667f, -0.1897565275f, -0.0161396451f, -0.5330590010f, -0.3432305753f, -2.3037126064f};
-
-
-const float fc2Weights[32][32] =
-{
-    {0.7028352022f, -0.2518471181f, -0.3251887262f, -0.1293392181f, -0.0480363145f, -0.2164561749f, -0.1114568934f, 0.0847787485f, 0.2538841367f, 0.0906840563f, 0.2075847685f, 0.0800402164f, -0.2112799883f, -0.3601601422f, -0.2693049312f, -0.3371258378f, 0.1860402822f, 0.1293898672f, 0.5152844191f, -0.0005433710f, -0.3649958968f, -0.5045146346f, 0.7517039180f, 0.0467921384f, -0.0150561901f, -0.2420729399f, -0.2145421207f, -0.7155304551f, 0.1590870321f, -1.1922683716f, 0.1453628838f, -0.3018942773f},
-    {0.4329630733f, 0.2430671006f, -0.8269199729f, -0.1718535721f, 0.6189324260f, -1.1788749695f, 2.3799111843f, -1.0581681728f, -1.0104187727f, -0.6895720959f, 0.6333325505f, 0.1227911711f, -1.3420652151f, 0.0161968544f, -0.9277707934f, -1.6182627678f, 3.4310810566f, -0.0272786003f, 0.9764053226f, 1.7808814049f, -1.4981951714f, -0.2589629292f, 0.0806718767f, -0.0988150090f, 0.0121101709f, -3.1666336060f, -0.9463910460f, -1.3826171160f, -0.1921611279f, -0.4246963561f, 0.0819954872f, -1.1248966455f},
-    {0.3439708948f, 0.1729307026f, -0.4266345203f, -0.1223015115f, -0.2364580184f, 0.0005747200f, 0.1383670121f, -0.0798279345f, -0.0409396328f, 0.2926900089f, 0.4414319098f, -0.0774125233f, -0.0639059693f, 0.0000417514f, -0.3764672279f, -0.2038611472f, 0.1025535613f, -0.2993794680f, 0.0265453998f, -0.0355118513f, 0.2918146253f, -0.4358098209f, 0.0194903836f, -0.0299784839f, -0.2588784993f, -0.2383993119f, -0.2728497982f, -0.3301697373f, 0.1411939114f, 0.1025134176f, -0.3637280166f, 0.1707316041f},
-    {-0.3416239917f, -0.1238532588f, 0.0436132401f, -0.3475806415f, -2.6594336033f, -0.9691415429f, -2.7146120071f, -0.4172722399f, -5.5016303062f, -3.0360279083f, -0.4493477941f, -0.3950566947f, -1.1143312454f, 0.0175548382f, 0.5904526711f, -2.4258356094f, -6.8880228996f, -0.3979260027f, -0.5621471405f, -8.3594980240f, -0.5225185752f, -0.0404085517f, 2.6085379124f, 0.0378435813f, -0.0098032495f, 2.7800383568f, 2.0576617718f, -1.8591514826f, -0.7343788147f, -0.4695331156f, 0.1112806350f, 1.4430732727f},
-    {-0.0525006801f, 0.3783730865f, 0.1891198754f, 0.0213207081f, -0.2073568851f, 0.1397801638f, 0.0859736055f, 0.2507794499f, 0.1215454638f, -0.1463970691f, 0.1565153897f, 0.0612230189f, 0.2093681097f, 0.1229698434f, -0.0846480504f, -0.2200313061f, 0.2009770274f, 0.0123162027f, -0.0025048242f, 0.0691637024f, -0.0181692336f, 0.2928285897f, -0.0372800417f, -0.3242078722f, 0.2153842747f, 0.0912938938f, 0.0259480923f, 0.2408835292f, -0.0780527815f, -0.0694554597f, -0.5314068198f, -0.3713911176f},
-    {-0.3659447432f, -1.0637227297f, -0.6574027538f, 0.1915589571f, -0.0923616737f, -0.6854819655f, -0.7665759921f, -0.3916678429f, 0.0046403264f, 0.1432497799f, -0.9219073057f, 0.3549784422f, -0.1066768840f, 0.0990313441f, -0.9819003344f, -0.6210372448f, 0.1118606776f, 0.2755944431f, -0.8569695354f, -0.1976287961f, -0.2853681743f, -0.6686331034f, 0.2660720646f, -0.1277324557f, 0.0465747900f, 0.4344924688f, -0.2714323103f, 0.2368822843f, -0.0763014555f, -0.2818318605f, -0.3344349861f, 0.1119496822f},
-    {-8.4284162521f, -0.5330117941f, 0.2430908680f, -0.9680790901f, 17.7502479553f, -11.1728906631f, -9.4553775787f, 6.4060249329f, -19.9450950623f, -9.8665761948f, -0.4943230152f, -0.0203626100f, 3.4065568447f, 0.1984576881f, 0.1283037961f, -1.1749160290f, -14.7431478500f, -0.9983112216f, -2.9252219200f, -6.1029782295f, -3.7169921398f, 0.0391248949f, -5.0480856895f, 0.1063139364f, -0.0771418661f, 9.4767627716f, -4.7266154289f, 2.5198585987f, -0.2968094051f, 0.0457970649f, -0.0931691378f, 5.0110268593f},
-    {-0.3695061207f, 0.1182122529f, -0.1756125242f, -1.9989162683f, -0.1902875155f, -0.5620827079f, -1.4479638338f, -0.0801911950f, 0.1420777738f, -1.3847892284f, -0.7704963088f, 0.0931626707f, -0.1455746740f, -0.2081220299f, -0.1101334319f, -0.1995007843f, 0.1743076593f, -0.2777135074f, -0.5316527486f, -0.7597036958f, 0.1699331254f, -0.0814936012f, -0.1331967264f, -0.1476776600f, -0.1340599060f, -0.1023281068f, -0.0449587218f, 0.5310350657f, -0.4073688686f, -0.1218718365f, -0.3036770225f, 0.5314686298f},
-    {-0.6574068069f, -0.4164880514f, -0.0907117650f, 0.0752569512f, -0.1371883452f, -0.0603592172f, -0.4597431421f, 0.1653049141f, -0.2182130367f, -0.1304199547f, 0.1074393615f, -0.0336866528f, -0.2670994699f, -0.0481679738f, -0.0528879240f, -0.0296069179f, 0.2772396803f, -0.0399867371f, 0.0935128778f, -0.2692438662f, 0.1292352676f, -0.4318626225f, 0.0235706642f, -0.0248435289f, -0.0899237916f, 0.2455069125f, -0.3291662335f, -1.0510460138f, 0.0582070835f, -0.0467071161f, 0.2144248039f, 0.2426371127f},
-    {-0.3157160878f, -0.4219513237f, 0.0111433808f, 0.0273274425f, 0.2885468304f, 0.4742646515f, 0.4801633060f, -0.0572001375f, -0.3388701677f, -0.6163141727f, -0.3216553330f, -0.1361154020f, -0.2719146311f, -0.0309467372f, 0.0720715001f, -0.5097295642f, 0.1592271924f, -0.1735037714f, 0.2940475941f, -0.3154088259f, 0.1547768563f, -0.3129318058f, -0.4059058130f, -0.1461700797f, -0.2799991965f, -0.2784145772f, 0.2787654102f, -0.7949113846f, -0.3778579533f, -0.0571825393f, 0.3065904379f, 0.5446800590f},
-    {-11.8102788925f, -0.5548009276f, -0.4933342040f, -0.0146987140f, 14.7732305527f, -3.3242385387f, -4.3986229897f, 0.9681160450f, -11.3604030609f, -8.9482917786f, -0.5679091215f, -1.4008077383f, -12.8347654343f, -0.3398197591f, -2.0602822304f, -10.3154926300f, 3.3564658165f, -0.3715273142f, -1.1404612064f, -0.5240572095f, -5.8742475510f, -0.8057600856f, -1.5432174206f, -0.0802463219f, -0.1602455229f, -0.8414320946f, -4.2538943291f, 4.6012043953f, -0.2584485114f, -0.3790409267f, -0.7869391441f, 0.8633705378f},
-    {-1.0586528778f, -0.1419243515f, -0.0970374048f, 0.2961471677f, 3.8987035751f, -2.2678067684f, -1.4299131632f, 0.7676140070f, -9.9476909637f, 5.8023109436f, -3.9433639050f, -0.4502748251f, 0.8263508677f, -0.2590869963f, -0.0040865224f, -2.3295035362f, 0.8782870770f, -0.6496896148f, 0.4475262761f, 2.5988130569f, 0.7252852321f, -0.4031449258f, -0.9870574474f, -0.3840217292f, -0.2451169640f, 1.9900392294f, 1.5628668070f, 0.8736712933f, -0.0124829793f, 0.0401136354f, -0.3562023640f, 0.2699736655f},
-    {-0.0120646609f, 0.3022353053f, -0.1795586795f, -0.0325300470f, 0.1483502388f, -0.3772916794f, -0.0676282048f, 0.2362668216f, -0.0225414429f, 0.4359962046f, -0.0775927231f, -0.0436186716f, -0.4518926442f, 0.1925306767f, -0.0252785441f, -0.2602519393f, 0.0216296762f, 0.0660978556f, 0.0655388087f, 0.1850759685f, 0.1910172254f, 0.1433989257f, 0.0925811827f, -0.1367113143f, -0.3113737106f, 0.0142191835f, -0.4499391317f, -0.6342935562f, 0.0016022279f, -0.0294085406f, -0.4484280348f, -0.1730211675f},
-    {-1.0707663298f, 1.4295915365f, -1.2444705963f, -0.1815876514f, -0.2770722806f, 0.3313229382f, -0.0980075672f, -0.4719111025f, 0.2169308513f, -0.4073899686f, -0.3677752912f, 0.1686977446f, -0.3691555560f, -0.0566529781f, 0.2436147034f, 0.0352467895f, 0.2247329205f, 0.1287635118f, 0.2276323736f, -1.1428972483f, 0.0267670061f, -0.3803151846f, 0.1016131118f, -0.3375857770f, -0.3619975746f, -0.0580476969f, -0.2426059097f, -0.2319097221f, 0.1949766576f, -2.0283784866f, -0.1499034166f, 0.3203954101f},
-    {-9.3854074478f, -0.0246858224f, -0.8499143124f, -0.2564937472f, -9.3853092194f, -0.6312729120f, -5.0737833977f, 1.9478274584f, 2.6517164707f, 2.3842592239f, -5.2550106049f, -0.3242772520f, 0.8110681176f, -0.3066453636f, -1.3071999550f, 2.7409677505f, 4.5771627426f, -0.5061553717f, -1.4946135283f, 3.5521233082f, -2.9335832596f, -0.5765511990f, 0.0422952175f, -0.1721477956f, -0.2773832083f, 0.2279552370f, -5.2490863800f, -0.7335987091f, -0.1851830781f, 0.0654127821f, -2.0507786274f, 0.3237775564f},
-    {-0.2222796977f, -0.3723528683f, 0.0691528544f, -0.3326320350f, 0.1267025322f, -0.0477940738f, 0.0229985509f, -0.0779023990f, 0.1464604437f, -0.1281969845f, 0.0605426840f, 0.0904146507f, 0.0433506742f, 0.1155809388f, 0.1346668303f, 0.0832128003f, 0.0945276693f, -0.2366465777f, -0.0344646983f, -0.0648121014f, -0.1341129988f, 0.0577474162f, -0.1170159131f, -0.0575251095f, 0.2074271739f, 0.1984294653f, 0.2561211288f, -0.4803146720f, 0.1444168389f, -1.3286510706f, 0.2394987047f, 0.0752803981f},
-    {0.1440746784f, -0.0171720050f, 0.4093155265f, 0.0168101508f, 0.3510462344f, -0.2379340529f, -0.1087244079f, 0.0702282861f, -0.2610715032f, -0.1717523634f, -0.0145056592f, -0.3848222494f, 0.1109743863f, 0.1177851558f, -0.1664983183f, -0.1298100203f, -0.1001451164f, -0.1466456205f, -0.3134774268f, -0.4611427784f, 0.0405314565f, -0.3590387702f, 0.2124959528f, 0.2551239133f, -0.1097064912f, 0.2295900434f, 0.1829279214f, -0.0514878631f, -0.1421726197f, 0.1224616244f, -0.2044410706f, -0.0536189489f},
-    {-1.1521507502f, -0.3603971601f, 0.0563264266f, -1.3970239162f, -1.4270666838f, -2.8226051331f, -3.5476942062f, -2.5657920837f, -0.5939258933f, -5.4976310730f, -4.0049266815f, -0.5885877609f, -0.1167592034f, 0.0184592586f, -1.5224845409f, -4.8727149963f, 3.0517530441f, -0.1223363504f, -0.2621241212f, -3.8385307789f, -2.8868563175f, -0.3684002161f, -0.3500567973f, 0.1355150640f, -0.4692768753f, -6.4998006821f, -1.3951023817f, -0.6174461842f, -0.1671719849f, -0.5628139973f, -0.2307008952f, 8.5936250687f},
-    {-0.3384640515f, 0.2626960576f, -0.2724999785f, -0.3156481981f, 0.1593002975f, -0.0599933714f, -0.5736973286f, -0.0508794934f, 0.0508684218f, 0.1900248677f, -0.2414632142f, -0.0151213873f, -0.0642893165f, -0.0736586526f, 0.3415043652f, 0.2434819192f, 0.1858230978f, -0.0688576773f, 0.0958920494f, 0.0314638801f, -0.2458027154f, -0.5904662013f, 0.0937323049f, 0.1025661081f, -0.1099876687f, 0.1343368292f, -0.2600298226f, 0.3431377113f, -0.2248598337f, -0.0926240832f, -0.0758377016f, -0.0951057747f},
-    {-10.8200063705f, -0.2333872318f, 0.0629985109f, -0.5098025203f, 5.5954618454f, -0.0266051386f, 1.0398765802f, 2.6189880371f, -0.8284100294f, -1.0288726091f, -19.0959739685f, -0.4239546657f, 2.2480175495f, -0.3452661932f, 0.5333261490f, 3.0123581886f, -0.5058676600f, -0.5662975311f, -1.4740906954f, 2.3249070644f, 1.9839859009f, 0.0691346005f, -8.4895219803f, -0.0517179966f, -0.3145155013f, 0.0487585813f, 0.5590797067f, 1.1825811863f, -1.1296035051f, -0.4631800354f, 0.0042153052f, 4.9763317108f},
-    {0.8741941452f, 0.2847254574f, -0.0165548436f, 0.0214925446f, 0.0281693470f, -3.5478408337f, 5.1184897423f, -1.0630372763f, -0.3170503676f, -7.8191223145f, -0.7177888751f, 0.0192223061f, -6.6766848564f, 0.0839038268f, -4.0633893013f, -0.6456161737f, 1.9720232487f, 0.5165043473f, 0.1431490332f, 3.3671495914f, -2.0822234154f, -0.2774647176f, 3.5052692890f, 0.0942892134f, -0.1689221412f, -23.8342685699f, -5.3589863777f, -15.0655832291f, 0.1842747778f, 0.0636580139f, -0.8683874011f, -9.0183506012f},
-    {5.8148365021f, -0.0181446839f, 0.0218663495f, -0.6435270309f, -4.6314897537f, 1.0660556555f, -0.7006200552f, -1.7170858383f, -11.6315021515f, -1.8548130989f, 2.3387854099f, 0.1161770225f, -4.4872589111f, 0.0984403118f, 2.3550364971f, -1.3147095442f, -1.7980459929f, 0.2122275829f, 0.2098008990f, 0.8191279769f, -2.6600685120f, -0.9796918035f, 3.9549865723f, -0.3468909264f, 0.1095151901f, 2.1919665337f, 0.8045359850f, 1.2023586035f, 0.1413695663f, -0.4612948000f, -0.0958333239f, 0.5004554391f},
-    {-10.4134435654f, -1.6761026382f, -2.1109883785f, -0.8899091482f, 0.9217932224f, -0.1489774138f, -1.2502399683f, -0.2223093659f, 0.2842899561f, 0.2826307416f, -11.5551824570f, 0.2193767130f, 0.2752348483f, 0.0379434153f, -3.4593420029f, 0.2076737136f, 0.1856303215f, 0.2192035168f, -0.7756754160f, 0.0748336688f, 0.4596520662f, -0.6056127548f, -0.0012871140f, -0.3025635183f, -0.5217733383f, -0.4117718935f, -0.3294989765f, 0.2200052887f, 0.2029565573f, 0.2765416205f, -0.4646576941f, -0.0226360932f},
-    {-0.3001706302f, -1.0916574001f, -0.1813789010f, -0.1071366295f, -0.5642485023f, -0.4381268024f, -0.2561828494f, 0.2427830398f, -0.0773036852f, 0.1807658523f, -0.6189127564f, -0.5280420780f, -0.2278218716f, 0.2025674731f, -0.1903136224f, 0.2945006490f, -0.0809962600f, 0.1900134534f, -0.5843535066f, -0.1001444906f, 0.1827683747f, 0.2382754385f, 0.3387266994f, -0.3220880032f, 0.1786493659f, 0.0670973137f, -0.1765727550f, -0.0752916187f, -0.1503725797f, -0.1599606723f, -0.5018463135f, 0.0873332471f},
-    {-0.1636323780f, 0.1241440102f, 0.0475576073f, 0.1504822969f, -0.1423517764f, -0.1402205229f, -0.1181961671f, 0.3235143721f, 0.0912692472f, 0.3641940355f, -0.2788277864f, -1.0504397154f, 0.1891589761f, 0.1606433988f, 0.5445721745f, -0.1664946973f, 0.2456973046f, -0.3874065876f, -0.3910988867f, 0.1926820129f, -0.0088137649f, -0.4388289452f, -0.1335452497f, -0.0635569021f, -0.2904116213f, -0.0950278938f, -1.6520583630f, 0.0426815301f, -0.2116074115f, -0.2022051066f, -0.7181299925f, 0.1868721247f},
-    {0.0034010340f, 0.2792718112f, -0.4612697959f, 0.0420143865f, -0.2565088868f, 0.1659194678f, 0.2364908457f, 0.3249883950f, 0.1776097417f, -0.1085670367f, -0.1002039164f, -0.0526343510f, 0.1710232794f, -0.1661372930f, 0.1210514680f, 0.0878346786f, -0.3646489382f, -0.0152188586f, -0.1779934615f, 0.0524123646f, 0.2448512614f, -0.1211499125f, -0.1922912896f, -0.3014973998f, -0.2541061938f, -0.0387258530f, 0.1648880988f, -0.0043392545f, 0.2601354420f, 0.0364522561f, 0.1693428457f, 0.0329939090f},
-    {-0.2273397893f, -0.3498373628f, -0.0433691703f, -0.6629248857f, 0.1507195234f, -0.1990272850f, -0.1220994815f, 0.1678037792f, -4.3043742180f, -0.0007548440f, -0.3417711258f, -0.1170029715f, -1.3333457708f, -0.0142377056f, 0.5767194033f, 0.4187206626f, -0.0696189702f, 0.1859471947f, -0.3047918975f, 0.4045199156f, 0.2059099227f, -0.7050588131f, 0.1555795074f, -0.0812577009f, -0.3910994828f, -0.0145086460f, -0.0485258102f, -0.0066310908f, -0.1054076031f, -0.5957096219f, -0.2342762351f, 0.0350304283f},
-    {0.0149504738f, -0.0129147042f, -0.5884258151f, 0.0588622056f, 0.0843603089f, -0.0650319681f, -0.1795133650f, -2.2022593021f, 0.1941308379f, -0.2966808379f, -0.2518850267f, -0.2675821781f, -0.5772732496f, 0.1356097460f, 0.1572682261f, -0.4143220186f, -1.1145333052f, -0.1922410876f, -0.1281387955f, 0.0762870386f, -0.2557103634f, 0.0021827971f, 0.1089263260f, 0.2346664369f, 0.2724923491f, 0.0007219575f, 0.2104968876f, -0.0320289358f, -0.1094986573f, -0.0914234072f, -0.2628631294f, 0.1553151160f},
-    {0.7854294777f, -1.2805747986f, -0.4509948492f, -0.5390222073f, -7.2329969406f, -4.1658449173f, 0.9921216369f, -19.2893733978f, -0.0606014021f, -1.3520914316f, -2.8341577053f, -0.5397177935f, -0.4183989465f, -0.3706202805f, -4.5540037155f, 0.8042452931f, 2.8141219616f, -0.0732958987f, -0.5256106257f, -0.1778799146f, -1.4509961605f, 0.0517916530f, 0.2504193485f, -0.3624364734f, 0.0715965852f, -2.5190768242f, -20.3786926270f, -0.2376282662f, 0.0138650360f, -0.1234268844f, -0.4907496870f, -1.5723351240f},
-    {0.1309531778f, -0.4162278175f, 0.0861416981f, -0.0754620507f, 0.3168306947f, -0.3067710400f, -0.0102485251f, -0.1974389702f, 0.1207621619f, -0.0179167688f, -0.1688643098f, 0.0122922705f, 0.1167635992f, 0.0521559902f, -0.3053426147f, -0.3361171484f, -0.3374931216f, -0.2523073256f, -0.0067732469f, 0.0612988696f, 0.1934308410f, 0.0817848817f, -0.1016546264f, -0.1203937978f, 0.1065604016f, -0.1992163658f, 0.3401925266f, -0.0275416337f, -0.2688343525f, -0.0388706326f, 0.1756630838f, 0.4808184803f},
-    {0.4282532632f, -0.0259445906f, 0.3069318831f, -0.3668542504f, 0.3145154119f, -0.3847868145f, 0.2201283127f, -0.0448469892f, 0.1542371064f, 0.2123837769f, -0.1569138467f, 0.0211382862f, -0.2548147142f, -0.2737289667f, -0.3175657392f, -0.5842364430f, -0.0748774335f, -0.1540294737f, -0.1290857196f, 0.1770722121f, 0.1833150536f, 0.0081414832f, 0.2064791620f, -0.1276530772f, 0.1270321757f, 0.1417734623f, -0.2329282910f, -0.3905624747f, 0.2545000315f, -0.3035842478f, 0.0064902604f, -0.3016678095f},
-    {-7.5552926064f, -0.9295108318f, -0.2481519580f, 0.7649185658f, 1.9046311378f, -0.1586459428f, 2.0594851971f, -1.3088877201f, -1.2542868853f, -4.6562218666f, 6.3942017555f, -0.6503939629f, -0.7048695683f, 0.2057881355f, 6.5724821091f, 1.7823606730f, -2.8745467663f, -0.3315669894f, 0.9826352596f, -2.7425603867f, -1.9275763035f, -0.2427215725f, -0.5757774115f, 0.2017655820f, -0.1918183118f, -16.7525539398f, -0.7728430629f, 1.0563217402f, -0.4434075356f, -0.7302488089f, -0.2927037179f, 2.4355039597f}
-};
-
-
-const float fc2Bias[32] =
-{-1.4964368343f, -0.4743577838f, -0.4512416422f, -0.4922141731f, -5.1149101257f, 8.8500652313f, -15.7193136215f, -0.0800511166f, 2.7641799450f, 3.4012768269f, 0.0683615580f, -0.1829399616f, 5.3457593918f, -0.4106808603f, -16.5018920898f, -11.1810235977f, -8.3992519379f, -0.4962880909f, -0.9232602119f, -11.2513113022f, 2.3576202393f, -0.4867421985f, -3.7326567173f, -0.2190053761f, -0.3950267136f, -1.8300843239f, 7.8722882271f, -1.0580558777f, -0.4571195245f, -0.4307751656f, -0.3282840848f, -4.9084253311f};
-
-
-const float fc3Weights[32][32] =
-{
-    {2.1267073154f, -0.3118368387f, -1.0274667740f, -0.3681430817f, -0.2503374219f, -1.2477926016f, -1.3221302032f, 1.8165496588f, -0.1359463781f, -1.7964631319f, -0.0097085517f, -0.3852086663f, -3.3534936905f, -0.8397763371f, -0.4446444511f, -1.6745910645f, 3.2378242016f, -1.0022313595f, -9.9781904221f, 0.1582326442f, 0.5432472825f, 0.0471910089f, 5.9129772186f, -0.2324501723f, -4.1757726669f, -2.3055243492f, 0.0340588912f, -9.1570177078f, 3.9973771572f, -5.0203042030f, -0.4807384312f, -0.2449323237f},
-    {0.2926399410f, -0.3126508892f, -0.2911956012f, -0.6654194593f, -1.9371089935f, -0.5545672178f, 0.4246672988f, 0.3819307983f, 0.2514972389f, -0.4406578541f, -0.9821344614f, -0.3851819932f, 1.1248499155f, 0.0522659570f, 0.0575070083f, 0.4138618112f, -0.8817019463f, -0.6708815694f, 0.4475752711f, -0.0536939353f, 0.2634044886f, -0.7456439734f, 0.3254241347f, 0.7578423023f, -0.0451397039f, -0.3245976269f, 0.3969579041f, -0.3185728490f, 0.5564263463f, -0.6522557735f, -0.1052212864f, -0.4474395216f},
-    {0.1283721924f, -0.0823663324f, -0.0326546133f, -0.4661917388f, 0.2401303649f, -0.2880379856f, -0.1517074406f, 0.2752179205f, -0.1285577416f, -0.9086889029f, 0.0597265214f, 0.0445332415f, 0.0693372712f, 0.3575956523f, -0.2324096560f, 0.6556770205f, -0.2550288737f, 0.0115993991f, -0.1578786522f, -0.2495867163f, 0.5065354705f, 0.1579007357f, -0.3488564789f, -0.3209209442f, -0.1637986600f, 0.0921269283f, -0.0206926819f, -0.4815258384f, 0.1049012691f, 0.0869352594f, 0.0118843094f, 0.0378016829f},
-    {-0.1867472231f, -0.0983402878f, -0.2410253137f, 0.1428901106f, 0.5535175800f, 0.4710266590f, 0.7919189930f, -0.1014919057f, -0.0083244918f, 0.1866499782f, -0.2188591063f, -0.2367116362f, -0.3858147562f, 0.2450350076f, 0.1948615909f, 0.2732373476f, -0.7079418898f, -0.4475770295f, -0.0726167932f, 0.0758384317f, -0.0683538988f, -0.3337415755f, 0.6586756110f, -0.3540558219f, 0.1567319185f, 0.2762941718f, -0.1348233819f, -0.2360792607f, -0.0053496510f, 0.3404291868f, 0.0747083575f, -0.4553619027f},
-    {-0.7439086437f, -0.5030723810f, -0.0015834231f, -0.2131769657f, -0.1265947670f, 0.6866528392f, -6.0031766891f, -1.1717530489f, -0.8875666857f, -0.7657486796f, -0.4995601177f, -1.3289493322f, 0.5762737989f, -0.1594046950f, -1.7588888407f, 0.0773057416f, -1.0809998512f, 3.3575456142f, -2.1498513222f, -0.1858167648f, 1.1153688431f, -0.2805164158f, -2.4953896999f, -3.0942769051f, -4.6361846924f, -7.2082462311f, -0.2423758358f, 9.1199541092f, -1.9552760124f, 1.5397961140f, -4.8174195290f, -0.1466594040f},
-    {-12.2784042358f, 0.2199732214f, -0.1896684021f, -0.3432999253f, -0.2287421525f, 3.4079124928f, -1.9381728172f, -0.6183780432f, -2.6809947491f, -13.9357881546f, -0.3349778652f, -1.1822427511f, -0.4702038467f, -0.4513809979f, -4.1594862938f, -1.3155949116f, -2.8984313011f, 0.8626719713f, 0.9431877732f, 0.1867329180f, 1.8311676979f, 0.0268813707f, -0.2230326235f, 1.5717616081f, 4.5641150475f, -5.9756793976f, -0.3749696314f, 6.8801670074f, -0.5390860438f, 2.2278966904f, 1.6726257801f, -0.4617499709f},
-    {-1.1322755814f, -1.6074182987f, 0.3383499086f, -0.0879338682f, -0.3024970293f, -0.9650658965f, -0.7258930802f, -2.2438650131f, -8.0822734833f, -1.1866042614f, -0.1294266731f, -0.3437821269f, -5.2474503517f, -3.8555898666f, -0.9632506967f, -1.5082763433f, -2.1201927662f, 1.0083860159f, 0.9984830022f, 0.0244099908f, 1.3445159197f, 0.0025331527f, 0.3188533485f, -1.2511210442f, 4.4629726410f, -0.6043246984f, -0.2112076432f, 0.2277358621f, -0.4532658756f, -2.5137176514f, -0.6861492395f, -0.2597316206f},
-    {-11.9988451004f, -0.1067576185f, -0.2066107839f, 0.0828568414f, -0.3275893927f, 2.5277581215f, 3.2627487183f, -5.6365389824f, 5.3499884605f, -3.2246093750f, -0.2226941139f, 0.1935922205f, -4.1013288498f, 0.0029573692f, -1.5204852819f, -4.4882822037f, 4.6080155373f, 4.0036969185f, 0.9089803696f, -0.2275806665f, -2.7591450214f, -0.1230073869f, 2.7025136948f, 5.8001189232f, -2.8862383366f, 1.4778681993f, -0.5003541112f, -8.5913944244f, 0.0913238823f, -3.5371487141f, 0.1153692529f, 0.0406519361f},
-    {-2.5129640102f, 0.3217219710f, -0.0095533794f, 0.1789117903f, -0.0394163057f, -12.0436925888f, 7.2080974579f, -0.6646071076f, 7.5206618309f, 3.8631877899f, -0.1600254625f, -0.8080578446f, -0.5643880963f, -0.0537163280f, -0.5046658516f, 0.4704113901f, -1.0934047699f, 1.3934750557f, 1.6021199226f, 0.2552498877f, -2.1392021179f, -0.1866302639f, -1.9532939196f, 4.3028411865f, -2.0663371086f, -1.1156477928f, 0.0436181799f, -4.5952415466f, -4.1031899452f, -4.2674293518f, -1.3652423620f, -0.3086400926f},
-    {-2.4256632328f, -0.8204917312f, -0.0080023678f, 0.0641652644f, 0.1045749411f, -6.7571711540f, -5.8372755051f, -2.5383017063f, 1.3260424137f, 0.7004460692f, -0.2244220227f, 0.5836414695f, -0.1815587580f, -0.3951809406f, -0.1336556673f, -4.6909227371f, -6.2935724258f, 3.7900424004f, 2.5683553219f, -0.1162255332f, 4.1622815132f, 0.1073398590f, -5.1608195305f, -8.2401542664f, -5.8381042480f, -7.4464135170f, -0.1182934269f, 3.2788193226f, 1.4351627827f, 6.3681168556f, 5.1041111946f, -0.1208729222f},
-    {4.9961571693f, 0.0185269248f, -0.2817080617f, -0.0505358987f, -0.2017169148f, 0.5220227242f, -9.6225223541f, 3.5301513672f, -1.2750257254f, -16.6149158478f, -0.2861652076f, -0.1112170741f, -2.9041886330f, -0.7071534991f, -1.5988026857f, -8.9261875153f, -14.3294153214f, 0.6273730397f, 1.6051287651f, -0.1642416716f, -3.4014506340f, 0.0300757475f, 6.7346963882f, -1.6067913771f, 1.6891504526f, -3.9646015167f, -0.0779341832f, 4.6224765778f, -0.5721715689f, -5.5780882835f, -0.1785165519f, 0.0633503497f},
-    {0.1060310677f, -0.5470390320f, -0.2241495699f, -0.4295381904f, -0.0263416618f, 0.2030417621f, -0.1120280996f, -0.0827973634f, -0.3317763507f, 0.0817295536f, 0.0069632535f, -0.1319767535f, 0.9747350216f, 0.2557510436f, 0.1425391138f, 0.1720905155f, -0.0782878473f, -0.0193020143f, 0.1281307042f, -0.2256549150f, 0.1310895681f, -0.5182238221f, 0.3424457908f, -0.1979330033f, 0.1891248226f, -0.0348026715f, 0.0921394229f, -0.0947700441f, -0.0883120522f, 0.0940800980f, 0.1856457591f, -1.7943713665f},
-    {-4.0017580986f, -0.9870634079f, -0.3691159189f, -0.4045964777f, -0.9596207142f, -4.4315557480f, -2.6966378689f, -2.4839437008f, 2.0009076595f, -2.3518767357f, 0.0318453647f, -0.5925750136f, -8.7461175919f, 1.2080634832f, -0.9580079317f, -1.3870022297f, -2.9447979927f, -4.2511048317f, -0.7817761898f, -0.3507715464f, 0.2093277872f, -0.2879707217f, 0.9649841189f, -10.7083244324f, -7.3718595505f, 1.7227441072f, -0.2694068253f, -2.7931368351f, 1.4890755415f, 0.6705868244f, 1.4951418638f, -0.1209532097f},
-    {-0.2123307288f, -0.3103626966f, -0.2075639665f, -0.0577281974f, -0.0194090679f, 0.2053133696f, -0.0279764533f, -0.1785610318f, -0.0686176419f, 0.2033739239f, -0.1825354397f, -0.0740804151f, 0.2677070498f, 0.2260797173f, -0.3779144287f, 0.0846814588f, 0.2516773045f, 0.1258523315f, 0.0956230313f, -0.1808306128f, 0.3722532988f, -0.1553636193f, -0.0801539496f, 0.2497462928f, -0.1287349761f, -0.1542355567f, -0.2239110470f, -0.0359012038f, 0.0341420211f, 0.1991766691f, -0.1235702857f, -0.0031303430f},
-    {6.9049019814f, -0.6181007624f, -0.1168652996f, -0.3459298015f, 0.1081510484f, -0.6562979817f, 2.9601066113f, 4.0602450371f, -0.7127177119f, -0.0100999130f, 0.5118251443f, 0.2948195040f, -14.6811800003f, -0.1756128967f, -2.3726568222f, -2.2973806858f, 0.2021040469f, 3.7788739204f, 0.0394109413f, -0.5389692783f, -2.5069026947f, -0.1306917369f, 1.1527950764f, -4.1568374634f, -5.4282240868f, 1.0624045134f, 0.2346842736f, -7.4216456413f, -10.1741409302f, -5.2678761482f, -1.5980980396f, -0.4154963493f},
-    {-5.9673295021f, -0.4644198418f, -0.4291569293f, -0.2272513807f, -0.4974555075f, 2.6671023369f, -6.5609960556f, 0.3609015644f, 10.3618955612f, -2.0600147247f, -0.5190568566f, -0.2184191942f, -1.4022922516f, -1.4489669800f, -4.0840444565f, -4.2195286751f, -0.4894198477f, -2.7413768768f, -6.3571834564f, -0.1267714947f, -0.8546049595f, -0.2224794775f, -0.3131478429f, -0.9897703528f, -6.1158819199f, 0.3078280985f, -0.4307037592f, -14.2193670273f, -0.3561481237f, -9.1594533920f, -0.7577811480f, -0.3548552096f},
-    {-6.2391667366f, -0.1269952357f, -0.2936189175f, -0.9923886061f, 0.0176051259f, -1.9780510664f, 0.0654375777f, -1.7040052414f, -0.1822838187f, -1.7437926531f, -1.0801513195f, -0.7526912093f, -5.0536708832f, -0.7413324714f, -2.9236466885f, -6.1675448418f, -0.9600590467f, -2.5335817337f, -1.1323231459f, -0.8290102482f, 0.5944318175f, -0.0634517670f, 0.9594070911f, 1.3713343143f, -3.9021794796f, -0.7598190904f, -0.7681146860f, -1.0350772142f, 0.8626580238f, -1.0701311827f, 0.6357883215f, -0.1052244753f},
-    {0.0669496581f, 0.1127517670f, -0.0209338553f, -0.0054164180f, -0.1271978915f, 0.0695113614f, -0.3660996258f, -0.7821827531f, -0.2065943033f, 0.2308539152f, 0.4742313325f, 0.1034071371f, -0.0490562394f, 0.4140049219f, -0.0322051346f, -0.0112497425f, 0.0862999111f, -0.1826869249f, -0.0142615261f, -0.0710267127f, 0.0182412416f, 0.0835287198f, -0.0485102497f, -0.1113768369f, -0.3792001903f, -0.0910689160f, 0.0768057555f, -0.4180118442f, -0.0000109978f, -0.0632229522f, 0.0589419529f, 0.1596196741f},
-    {0.3151653707f, -0.4588074088f, -0.3387854099f, 0.1461272687f, 0.0001908212f, -0.0586236343f, -0.8286342025f, 0.2795176804f, 0.4374869168f, 0.0628984645f, -0.2587254047f, 0.2453358322f, -0.6134222746f, 0.0773593485f, 0.2486565858f, -0.5328485370f, 0.6034097075f, -0.3007964790f, -0.2765316963f, -0.2508219779f, -0.2341709882f, 0.0143393278f, 0.1970690787f, -0.2759173810f, -0.7320910692f, -0.2636430860f, -0.0779949948f, -0.5681775808f, 0.0010494586f, 0.3486377001f, 0.5590596199f, -0.1548434943f},
-    {0.2904222906f, 0.2120339572f, -1.5837268829f, 0.1351595223f, -1.0482976437f, 0.6585662961f, 1.2837238312f, -8.8914051056f, 0.1900334507f, -6.2876024246f, -1.1750501394f, -0.9563908577f, -6.0589551926f, -0.0554546751f, -2.5099968910f, 0.2854703367f, 0.5719563365f, -0.4985203147f, -0.8484790921f, -0.2796662748f, -2.6506903172f, -0.0737289563f, 0.1099997908f, -6.8096685410f, -9.5710783005f, -3.5462114811f, -0.3454555869f, -4.1742453575f, -6.2087631226f, 1.0913355350f, -5.9188799858f, -0.0447143987f},
-    {-0.1762033552f, -0.9464151859f, -0.3476518989f, -0.1438073069f, -0.1068934649f, -2.6918101311f, 13.4009866714f, -0.0717068240f, -1.2019355297f, -2.8347525597f, 0.1641165912f, -0.0925235450f, 4.8379797935f, 2.1199939251f, -0.3954177201f, -1.5702075958f, 0.0889840126f, -1.7895882130f, 1.9846193790f, -0.0691496804f, -6.4447302818f, 0.1634601355f, 9.6448106766f, -0.7868277431f, 2.1918652058f, -0.5956330895f, -0.4198028147f, 2.8396642208f, 3.0820310116f, 3.5346202850f, 1.1110432148f, 0.1005667076f},
-    {0.2411281914f, 0.1411192566f, -0.0250760857f, -0.1230037659f, 0.6549731493f, 0.2007940114f, -1.5664811134f, 0.3535908461f, 0.6514756680f, -0.3146148622f, 0.0244260952f, -0.1043881625f, 0.5631551743f, -0.1026171371f, -0.2293619066f, 0.0751082525f, 0.1610344499f, 0.3488910794f, -0.3952502310f, 0.0538705476f, -0.1643067747f, -0.0639704242f, 0.1503835022f, 0.1106679291f, 0.3851269782f, 0.3049305081f, 0.2240979224f, 0.4056433737f, -0.3284457624f, -0.4254478812f, -0.0813698918f, -1.5299969912f},
-    {4.3450713158f, -0.4628526270f, -0.1491935551f, -0.2041818649f, -0.5786065459f, -1.2504427433f, -0.2758893967f, -0.3101694584f, 2.9785020351f, -4.7803659439f, -0.3506944776f, -0.9885157347f, -7.2183914185f, -0.1302880943f, -2.8009421825f, -0.6073452830f, -0.8039718270f, -12.0689706802f, -0.7140820026f, -0.3246946931f, 2.0155956745f, -0.3901022673f, 3.3020424843f, -0.5596037507f, 0.5555099249f, 0.2735705674f, -0.2914109826f, -14.4507541656f, -17.0698108673f, -4.3910245895f, -5.2524790764f, -0.2280410379f},
-    {-0.0042843656f, -0.1069235355f, 0.0691635162f, 0.0867756158f, 0.2937769592f, 0.0736219883f, -0.1853623986f, -0.1654763520f, 0.2185866535f, 0.1437660605f, -0.2649201155f, -0.1712868661f, -0.0538765527f, -0.1329404116f, 0.0423769951f, 0.3043175936f, 0.2732996941f, 0.0199668072f, 0.2512804568f, 0.1751035899f, -0.2383348942f, -0.2895450890f, -0.0679779947f, -0.2974975705f, 0.1156523153f, -0.2165189683f, 0.2342301458f, -0.1439705789f, 0.3337920308f, 0.2263115346f, 0.2996229827f, 0.1000883803f},
-    {-0.0903539732f, -0.2626895308f, -0.2928705215f, -0.1522098780f, -0.1990660727f, 0.1050468609f, 0.0752090216f, -0.0103869336f, 0.0290090404f, -0.2140946537f, -0.3905937076f, -0.0324644707f, 0.1388606578f, -0.0920412987f, 0.2089990675f, -0.0512218550f, 0.0448999256f, -0.0192409158f, 0.0232043508f, -0.1123628542f, 0.1863525957f, -0.1689641625f, -0.0414763764f, -0.0482044294f, 0.1756522357f, 0.3836692274f, -0.2097899765f, 0.0034760062f, 0.3211987317f, -0.1981586516f, 0.0146787502f, 0.1057996526f},
-    {-0.5354076028f, -0.3253239095f, -0.1333258152f, -0.0967850685f, 0.0488308556f, 1.7821354866f, -2.4696009159f, -1.0925548077f, 4.8946523666f, -5.5363497734f, -0.0556999668f, -0.3859304488f, -0.4668995738f, 1.1459211111f, 0.0986527577f, -5.5068421364f, -2.1916167736f, 0.9777641892f, 1.2964626551f, -0.1227272600f, -2.6892449856f, 0.2693175077f, 2.4651014805f, -0.2143563330f, -8.1490335464f, -1.1221495867f, -0.2091181278f, -2.5353107452f, 1.5514105558f, -0.8118920326f, -0.0759748369f, 0.0815865844f},
-    {-1.1687350273f, 0.0523806289f, 0.1928860396f, -0.1731245518f, -0.0422611423f, -3.5962536335f, 2.1258342266f, 0.0682783425f, -0.6722916365f, 0.6611677408f, -0.4290418625f, -0.2772609890f, 0.0911868215f, -0.2093296349f, -2.5827946663f, 4.6710624695f, 2.0906822681f, 0.5159549713f, 0.3715068996f, -0.2340186238f, -0.4284092188f, 0.0376657546f, -5.7211947441f, -1.1870640516f, 2.2118372917f, -8.0735073090f, -0.0517498069f, -4.1637449265f, -17.3150138855f, 3.2159905434f, -5.9302067757f, -0.0362711921f},
-    {-9.7952108383f, -0.6339132190f, -1.7921937704f, -0.3130472600f, -0.7404204011f, -1.6049377918f, 0.1309763938f, -7.6337146759f, 0.4408401549f, 3.3682234287f, -0.7966508269f, -0.0931789875f, -4.3933353424f, -7.4179029465f, 0.2328766286f, -2.2703931332f, -11.0213117599f, -11.4555244446f, 0.8035743237f, -0.3479077518f, 2.3253028393f, -0.1451899409f, -0.3695332706f, -16.2041606903f, -3.2792532444f, 0.6592780352f, -0.4765171707f, 1.1891194582f, -0.2045537680f, -0.2613292336f, -7.1603960991f, -0.0806212798f},
-    {-0.2485560924f, -0.5306785703f, -0.2062491179f, -0.1363802254f, -0.0375753008f, -0.2761560082f, -0.2498460114f, -0.0567790791f, -0.4666529298f, -0.0051496779f, 0.0906768441f, 0.3301909864f, 0.0531814285f, 0.2584980726f, 0.0183127709f, 0.1766013205f, 0.1447227001f, -0.1579496711f, 0.0043631280f, 0.1031887531f, 0.0396930017f, -0.0095448559f, 0.1428825110f, -0.0905142054f, 0.0705604851f, -0.7297466397f, 0.1558683962f, -0.0418609716f, 0.1233214065f, 0.1279907525f, 0.1352362633f, -0.0770157427f},
-    {-0.0049988753f, 0.1279107481f, 0.0449896753f, 0.6752018332f, -0.2889954150f, 0.4311439991f, -0.3787408769f, -0.4591981173f, -0.1940811723f, -0.0184744801f, 0.0441148244f, -0.1790693253f, -0.7149969935f, -0.0285517797f, -0.1489535421f, -0.5780433416f, 0.5109094977f, 0.6176788211f, -0.3211218417f, 0.1074986085f, 0.2505583465f, -2.5777461529f, 0.3480081260f, -0.8229233623f, -0.6535633206f, 0.5414181352f, 0.8112808466f, -0.3283018768f, 0.4965822697f, 0.8790674806f, 1.1569755077f, 0.2319966406f},
-    {-0.2767451704f, 0.2554717064f, -0.2503568828f, 0.2911026180f, -0.0896157026f, 0.0468162894f, -1.6308424473f, -0.2833184302f, -1.0214830637f, -0.1677722186f, 0.0416059718f, -0.0212873127f, -1.5793803930f, -0.0655662566f, -0.2267671078f, 0.3937034011f, 0.0103700571f, 0.1117373928f, 0.3512356877f, -0.1141320691f, 0.2422832102f, -3.8471693993f, -0.6802755594f, 0.1919109225f, -0.2617484927f, -0.5890094638f, 0.0800860524f, 0.1804080307f, -0.0786418170f, 0.2081480175f, -0.2514747679f, -1.6461024284f},
-    {-4.4110560417f, 0.0582131147f, -0.2296621650f, 0.0098368116f, -0.3111234009f, -0.7137008309f, 2.6443011761f, -5.5492348671f, -4.7353625298f, -1.0368877649f, -0.4160888791f, -1.2896853685f, 3.9724817276f, -10.6603651047f, 2.2762250900f, -3.0436787605f, -8.0274572372f, -1.4319875240f, -3.7743313313f, -0.0725755915f, -0.0383329280f, -0.3231822848f, 0.4311948717f, -2.6726655960f, -0.5350571871f, -1.4796928167f, -0.1884928495f, -5.7419962883f, -0.9663215280f, 0.1886327565f, -8.7959461212f, -0.1736448258f}
-};
-
-
-const float fc3Bias[32] =
-{-11.7193555832f, -0.6158396602f, -0.5767308474f, -0.4492973387f, -0.4815680087f, -1.5243271589f, -28.5404338837f, 4.9779524803f, -48.1950645447f, 5.1014094353f, -0.8038403392f, -0.7135465741f, 7.9977378845f, -3.9328453541f, -22.0385246277f, 0.3583163023f, 1.1570060253f, -13.7082090378f, 2.4488532543f, -0.4611511827f, -7.0892620087f, -0.3534394205f, -4.1933374405f, -13.2186746597f, -16.0344810486f, 5.2995457649f, -0.7092553973f, -11.6922359467f, 5.9151840210f, -4.7581868172f, -5.7193002701f, -0.4998539686f};
-
-
-const float fc4Weights[32][16] =
-{
-    {0.1313932389f, -0.6613871455f, 0.0097358534f, -0.1018039361f, -0.4058391750f, 0.4376264215f, -1.2572003603f, -3.6500022411f, -0.4249550700f, -0.2536546886f, -0.8922878504f, -0.4847670496f, -0.5938788652f, 0.1258193851f, -0.2272187024f, -0.1410650164f},
-    {0.0492341593f, 0.0943892971f, -0.5367519855f, 0.2006352544f, -0.3140752912f, 0.3790577352f, 0.5460243821f, 1.5730974674f, 0.4506726563f, -0.3239695728f, 0.1827834398f, -0.2436640859f, 0.4437866211f, 0.3330729902f, -0.2623860538f, -0.2274987251f},
-    {0.3261446357f, -0.5036940575f, -0.0645739883f, 0.1797081381f, -0.0971707106f, 0.7298631668f, -0.4873719513f, -0.1038509980f, -0.8186036348f, -0.0861302018f, 0.0887329206f, -0.2477209568f, -0.4017193019f, -0.2803779542f, 0.5708403587f, 0.7890117168f},
-    {-0.4605375826f, -0.3520908654f, 0.4579149187f, -0.6693316698f, 0.3497380316f, 0.2658064663f, -0.1681881249f, -0.3272072375f, -0.0845411718f, 0.2394114435f, -0.0101059023f, 0.4046859443f, -0.7417863607f, 0.4150537550f, 0.0869048312f, 0.0200589113f},
-    {-0.7960591316f, 0.7115456462f, 0.5414740443f, 0.2834898829f, -0.1049120426f, 1.2471122742f, 0.9764256477f, -0.2314370126f, 0.4565517008f, -0.1377106160f, 0.0066791684f, -0.0875304118f, -0.3973374367f, -0.4656803310f, -1.2865468264f, -0.2754203975f},
-    {-0.1267081201f, -0.3586117029f, -0.4626653492f, 0.0707737282f, -0.4280650318f, 0.3731042147f, -1.5224643946f, -0.0001562309f, -0.5457718372f, -0.8392512798f, -0.3297198415f, -3.6849846840f, -0.3235607445f, -0.4571735561f, -2.5906243324f, -0.0216693431f},
-    {-0.1975702047f, -0.6478233337f, -1.0000374317f, -0.7892091870f, -0.6129112840f, 0.4033009410f, -1.0652874708f, -5.0731873512f, -0.8335696459f, -0.0490954109f, -4.0255231857f, -2.5913162231f, -0.0984867662f, -0.3165186942f, -1.6619985104f, -0.1019358039f},
-    {-0.0070813317f, -1.4723222256f, -0.1055053324f, -0.0588658154f, -0.0424999259f, -2.5744652748f, -1.9830906391f, 1.0004559755f, 0.7691277862f, -0.1811856925f, -0.2902224660f, -0.2105268091f, 1.2195326090f, -0.0701754391f, -0.4191590548f, 0.0362841599f},
-    {-0.1953794360f, -0.7642983794f, -0.5502391458f, -0.6046341658f, -0.0868658274f, -1.0930783749f, -0.4096246660f, -0.2226173431f, -0.9467145205f, 0.0366492942f, -1.0827271938f, -1.3997668028f, 0.1914290041f, -0.1873319149f, -0.3942896426f, 0.0503584519f},
-    {-0.0302787200f, -5.7183461189f, 0.0836869702f, -0.1351985931f, 0.1218350157f, -0.0037351907f, 0.9376742840f, 0.5179937482f, -1.2707405090f, -0.2760854661f, 0.9394776821f, -0.6231134534f, -0.5011545420f, -1.0431542397f, -0.3772255182f, -0.9186507463f},
-    {0.5151054263f, -0.1792936772f, 0.5858933330f, -0.0756824464f, -0.1400850862f, 0.3227327764f, -0.0678724647f, 0.6420112848f, 0.7452553511f, 0.0104118586f, 0.1161525100f, -0.9172155261f, -0.3198927939f, 0.0625916570f, 1.1083164215f, 0.5252248645f},
-    {0.0453939959f, 0.0149580799f, -0.6745067239f, 0.2029461563f, 0.2238395065f, -0.2415017635f, -0.0304794274f, 0.0479286723f, -0.3008814454f, -0.2162462026f, -0.3752769828f, 0.7910355926f, -0.1502458900f, 0.2189511806f, -0.4534323215f, 0.2343945056f},
-    {-0.0660357103f, 0.5701023936f, -0.7573124170f, -0.4243010581f, -0.3647210896f, 0.9252396226f, -2.1346116066f, -1.5707424879f, -1.0428055525f, 0.0289310124f, -2.2915866375f, -2.0114760399f, -1.4218503237f, -0.6069518924f, -3.6163923740f, -1.0943173170f},
-    {-0.3510537148f, 0.0646613464f, -0.1941086054f, -0.3585000038f, -0.6358650923f, 0.4817451835f, 0.1834470332f, 1.5077445507f, 0.1200657785f, 0.0157124996f, 0.0893150121f, 0.2747304440f, -0.8162755370f, -2.2185630798f, -0.7672387958f, -0.7171642184f},
-    {-0.2655964494f, -0.6097341180f, 0.2751416266f, 0.0416029692f, -0.3487569392f, -4.3967308998f, -0.0399694182f, -0.5985686183f, -0.3526073098f, -0.0124356151f, -0.7412110567f, 0.3568145633f, 2.2140672207f, 0.3069616854f, -0.3047554493f, -0.9152814150f},
-    {-0.3112998009f, -1.2923207283f, -0.2642335892f, 0.1059029102f, -0.6062253118f, 1.7047382593f, -0.2192801982f, -7.2568459511f, -0.6452113390f, -0.1957286149f, 0.4478967488f, 2.1868736744f, 1.0964838266f, -0.5934657454f, -0.9350823164f, 0.1335169673f},
-    {-0.1445592940f, -3.8598067760f, -0.0888997465f, -0.4536363184f, -0.0818780512f, -0.5499716997f, 0.8879400492f, 0.3036511838f, -2.1018016338f, -0.3131541610f, -2.5331704617f, -2.7884752750f, -1.5264098644f, -0.2974909842f, -0.3884962499f, -1.6347447634f},
-    {-0.1576830745f, -1.0188829899f, -0.1347837746f, 0.0133346859f, -0.1020436883f, 0.9006328583f, 0.4465196133f, -1.9912528992f, -1.1890530586f, -0.2928483784f, -1.1970791817f, -1.0793333054f, -0.6542327404f, 0.0027635724f, -0.7239921689f, -0.1350315958f},
-    {-0.0101720933f, 0.8065249920f, -0.2725579739f, -0.5590948462f, -0.6773648858f, -0.9759730697f, 1.1105380058f, -0.9370074272f, -1.0632905960f, -0.2358790785f, -2.6182923317f, -0.5131548643f, 0.3938317299f, -0.6527919173f, -0.5773690343f, -2.7888607979f},
-    {0.9983167648f, 0.0334987827f, 0.1105532423f, -0.4079656899f, -0.1334102899f, 0.1476265639f, 0.0125124194f, -0.2545211911f, -0.3887112439f, -0.3177830279f, -0.3589894176f, 0.1081808954f, -0.3721842766f, 0.0556281544f, -0.0388795845f, 0.2037374824f},
-    {-0.4827512801f, -0.2132459283f, -0.9375121593f, -1.3464082479f, -0.5630852580f, 0.0437785275f, -6.4064550400f, -2.0924882889f, -0.2812258899f, -0.5477465987f, -1.5397117138f, -4.5946688652f, -0.4861118793f, -0.7390319705f, -1.4275629520f, -1.5372459888f},
-    {0.0196762551f, -0.0118051507f, 0.2246439159f, -0.2613828778f, -0.5333120823f, -0.1223717630f, -0.1976639330f, -0.1101496667f, -0.0134799089f, 0.0676583350f, 0.2612578273f, 0.1215140149f, 0.2203066051f, 0.2337986380f, 0.1590026319f, -0.0366739817f},
-    {-2.0189671516f, 0.4665688872f, -0.8338592052f, -0.3301258981f, -0.4463534951f, -0.3213590384f, -0.2176769376f, -0.7976086736f, -1.4433599710f, -0.9578914046f, -2.3176016808f, -3.3081896305f, 0.1362658292f, -0.9325780272f, -7.5809931755f, -6.1999273300f},
-    {-0.2162996083f, -1.1839522123f, -1.3205081224f, -0.0846010596f, -0.2445309013f, -0.2086048126f, 0.8832997084f, 0.2233153582f, 0.6593582034f, -0.2484957576f, -0.5232675672f, -0.0044967751f, 0.1215033680f, -0.2973840833f, -0.0051185489f, 0.4212923348f},
-    {-0.3386851847f, -0.1535801440f, 0.2589180470f, -0.5164783001f, -0.0374755822f, 0.2122802138f, 0.2694437504f, -1.9194368124f, -0.4101884961f, -0.5941445827f, -1.8583507538f, -0.8249487281f, -1.2600407600f, -0.3161348701f, -1.8951197863f, -0.5458792448f},
-    {-0.5639970899f, 0.5414865613f, -0.9907631874f, -0.7151839733f, -0.3994543254f, -0.8817944527f, -0.5968976021f, -0.2674676180f, 0.4723920822f, -0.6860534549f, -1.4526910782f, -2.7732286453f, 0.6615548134f, -0.4297663867f, -0.3635542095f, -0.7897509336f},
-    {0.2338507622f, -0.0548883379f, -0.6124376655f, 0.3055166602f, -0.6472301483f, -0.0934592113f, -0.0791300610f, 0.0744187683f, -0.0494301580f, -0.0888935924f, -0.2844451666f, -0.3919087350f, -0.1899581403f, 0.0696015507f, -0.3865183890f, 0.1351541430f},
-    {-0.3884226382f, 0.6438637376f, -0.0235484447f, -0.4085457325f, -0.3678089380f, -0.3611978292f, -1.9616941214f, -0.6029480100f, -1.0217937231f, 0.0628737658f, 1.1272052526f, -0.7026557922f, -2.6631255150f, -0.0570945218f, -0.8197408915f, -0.0155918878f},
-    {-0.3045629263f, -1.2819237709f, -0.4050366580f, -1.0614979267f, -0.1641227305f, -1.2281309366f, -1.3765566349f, 0.6788370609f, -1.5758328438f, -1.0314040184f, -6.2401552200f, -3.9707064629f, 0.3741535544f, -0.5852952600f, 0.2783090174f, -0.1958702654f},
-    {-0.3730004132f, 0.3823136389f, -0.0965899155f, -0.1237012967f, -0.0356198289f, -0.2304152846f, -2.5172274113f, -0.2180451900f, -0.3073972762f, -0.2923755348f, -4.9185404778f, -3.6769018173f, -0.4479543269f, -0.0074586943f, 0.7228356004f, -0.3836637735f},
-    {-1.5993598700f, -1.3894209862f, 0.0539587401f, -0.3277336061f, -0.2855097055f, -3.6847970486f, -4.5063147545f, -1.4260863066f, -3.9039046764f, -1.1701396704f, -1.8200896978f, -2.4587380886f, 0.2993653715f, 0.0570639670f, -2.6918485165f, -1.4997549057f},
-    {0.1219597459f, 0.0154820858f, 0.3974801600f, 0.3066018224f, 0.1819918901f, 0.0161614101f, 0.0640004650f, 0.1952118576f, -0.2755865157f, 0.0411446691f, 0.0500190333f, 0.1724539101f, -0.1740499139f, 0.3841970265f, 0.3317279220f, 0.4177183211f}
-};
-
-
-const float fc4Bias[16] =
-{-0.5141325593f, -0.8478075862f, -1.0021078587f, -1.7178856134f, -0.9628381133f, 0.1455254108f, -3.6562759876f, 1.7083742619f, -8.0320730209f, -0.9758637547f, -3.8376631737f, -0.1638645679f, 3.8785557747f, -1.0468775034f, -29.1870574951f, -6.1543283463f};
-
-
-const float fc5Weights[16][8] =
-{
-    {-0.0377756208f, -0.0376064293f, -0.0772271156f, 0.5590630174f, -0.3684624434f, 1.0009193420f, 1.3445713520f, 0.5627732277f},
-    {-0.4041293859f, 0.1303688884f, -0.3772308826f, -0.8144312501f, -0.3996099234f, -0.8786695004f, -0.1038818136f, -0.6491676569f},
-    {-0.1548538208f, -0.4342842698f, -0.3643324077f, 0.7171005011f, -0.2240766436f, -0.3029537201f, 0.0784914643f, 0.2668318450f},
-    {0.0582836233f, 0.1667208225f, 0.1605709791f, -0.8526514769f, -0.0394123383f, 0.3542331457f, -0.0112714395f, -0.8478502035f},
-    {0.1886940002f, 1.4096331596f, -0.0266252328f, 0.1160744503f, 1.3083670139f, -0.4767547250f, -0.7634902000f, -1.2924789190f},
-    {-0.3800563812f, -0.2717871964f, -0.2375701517f, -0.8516035080f, -0.3561956882f, -0.5633419156f, 0.1343641281f, -0.8182802796f},
-    {-0.4533548355f, 0.4980821609f, -0.0113001689f, -0.6163378358f, -0.0099237291f, -0.0464528799f, 1.4334065914f, -0.1762148142f},
-    {-0.4913391173f, 0.1923918128f, -0.5117462277f, -0.5446628928f, -0.7394183278f, 0.1009852663f, -0.2048954815f, -0.3028838933f},
-    {0.0514116511f, -0.1429781914f, -0.2054089010f, -0.3337486684f, -0.9298329949f, -0.0842700675f, -0.9120995402f, -1.0035445690f},
-    {0.1642776728f, -0.1338489354f, -0.3949478865f, 0.2676415145f, 0.2334752530f, 0.1165231466f, 0.3371087313f, -0.0303090699f},
-    {-0.0316087417f, -0.3532579839f, -0.5091804862f, 0.0389130265f, -0.1303991377f, -1.4411540031f, -0.0542913377f, -0.6798272133f},
-    {-0.0166958608f, -0.3853805959f, -0.0088427681f, -0.1329081804f, -0.1517693698f, -0.7518531680f, -0.0679639205f, 0.1632424593f},
-    {-0.2969943583f, 0.2169936597f, -0.2704928219f, -0.5222067237f, -0.2086613327f, -0.5919396281f, -0.1555189341f, -0.5921875238f},
-    {0.3620398343f, -0.0771873146f, -0.4576121271f, 0.4077250361f, 0.1402812898f, -0.3508163095f, -0.0904585123f, 0.1637402475f},
-    {-0.3381429911f, -0.9172439575f, -0.2056993395f, -0.0469768532f, -0.0229178257f, 0.0243457071f, -0.1943701357f, -0.0122073563f},
-    {-0.2032035887f, -0.0370536968f, -0.2462941855f, 0.3719790280f, 0.1286954135f, 0.1040267125f, 0.2068356276f, 0.0400213264f}
-};
-
-
-const float fc5Bias[8] =
-{-0.4388153255f, -2.2450299263f, -0.3559921980f, -0.9536105990f, -1.0454660654f, -0.8810431957f, 1.6276029348f, -0.6507120728f};
-
-
-const float fc6Weights[8][1] =
-{
-    {-0.5150820613f},
-    {0.1128230840f},
-    {0.0093749743f},
-    {-0.4838168621f},
-    {0.2524490356f},
-    {-0.4693250954f},
-    {-0.1718080938f},
-    {-0.4312117100f}
-};
-
-
-const float fc6Bias[1] =
-{0.0394757539f};
-
-
-#endif
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt b/source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt
deleted file mode 100644
index 55268027619bf2fbca74764613ca0ab583ae1383..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore KITGripperEtherCAT)
- 
-armarx_add_test(KITGripperEtherCATTest KITGripperEtherCATTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp
deleted file mode 100644
index 1cd0144f388fb8c25ff0f0a61fec8060906c8f41..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    ImagineRT::ArmarXObjects::ImagineEtherCAT
- * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::KITGripperEtherCAT
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h>
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(torqueEstimationPerformanceTest)
-{
-    TIMING_START(NN_Calc);
-    int iterations = 1000;
-    for (int i = 0; i < iterations; i++)
-    {
-        estimateTorque(rand() % 800, rand() % 3000);
-    }
-    TIMING_END(NN_Calc);
-    BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
deleted file mode 100644
index 94e91cdf9d477c838b7b18f47b56cbcfbb4a5be3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
+++ /dev/null
@@ -1,22 +0,0 @@
-set(LIB_NAME       NJointControllerGuiPluginUtility)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-set(COMPONENT_LIBS
-    SimpleConfigDialog
-    RobotAPIComponentPlugins
-)
-set(SOURCES 
-    detail/NJointControllerGuiPluginBase.cpp
-)
-set(HEADERS
-    NJointControllerGuiPluginBase.h
-    detail/NJointControllerGuiPluginBase.h
-)
-set(GUI_MOC_HDRS detail/NJointControllerGuiPluginBase.h)
-set(GUI_UIS)
-
-if(ArmarXGui_FOUND)
-    armarx_gui_library("${LIB_NAME}" "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h
deleted file mode 100644
index 6f3594264962d3dc003994ea46dc8c7566ee2586..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::NJointControllerGuiPluginUtility
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2020
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#pragma once
-
-#include "detail/NJointControllerGuiPluginBase.h"
-
-namespace armarx
-{
-    template<class Derived, class Proxy>
-    class NJointControllerGuiPluginBase:
-        public detail::_NJointControllerGuiPluginBase::Base
-    {
-    public:
-        NJointControllerGuiPluginBase(const std::string& ctrlClass) :
-            detail::_NJointControllerGuiPluginBase::Base(),
-            _ctrlClass{ctrlClass}
-        {}
-        ~NJointControllerGuiPluginBase()
-        {
-            deleteController();
-        }
-    public:
-        QString getWidgetName() const final override
-        {
-            return Derived::GetWidgetName();
-        }
-        QIcon getWidgetIcon() const final override
-        {
-            return Derived::GetWidgetIcon();
-        }
-        QIcon getWidgetCategoryIcon() const final override
-        {
-            return Derived::GetWidgetCategoryIcon();
-        }
-    public:
-        void createController() override
-        {
-            ARMARX_TRACE;
-            std::lock_guard g{_allMutex};
-            if (_controller || !getRobotUnit())
-            {
-                ARMARX_IMPORTANT << "No RobotUnit or controller already created";
-                return;
-            }
-            ARMARX_IMPORTANT << "Creating " << _ctrlClass << " '"
-                             << getControllerName() << "'";
-            _controller = Proxy::checkedCast(
-                              getRobotUnit()->createNJointController(
-                                  _ctrlClass,
-                                  getControllerName(),
-                                  readFullCFG()
-                              ));
-            ARMARX_CHECK_NOT_NULL(_controller);
-        }
-        void activateController() override
-        {
-            ARMARX_TRACE;
-            std::lock_guard g{_allMutex};
-            if (!_controller)
-            {
-                ARMARX_IMPORTANT << "No controller";
-                return;
-            }
-            ARMARX_IMPORTANT << "activating controller";
-            _controller->activateController();
-        }
-        void deactivateController() override
-        {
-            ARMARX_TRACE;
-            std::lock_guard g{_allMutex};
-            if (!_controller)
-            {
-                ARMARX_IMPORTANT << "No controller";
-                return;
-            }
-            ARMARX_IMPORTANT << "deactivating controller";
-            _controller->deactivateController();
-        }
-        void deleteController() override
-        {
-            ARMARX_TRACE;
-            std::lock_guard g{_allMutex};
-            if (!_controller || !getRobotUnit())
-            {
-                ARMARX_IMPORTANT << "No controller";
-                return;
-            }
-            ARMARX_IMPORTANT << "deleting old controller '" << getControllerName() << "'";
-            try
-            {
-                _controller->deactivateAndDeleteController();
-            }
-            catch (...) {}
-            _controller = nullptr;
-        }
-    protected:
-        Proxy       _controller;
-        std::string _ctrlClass;
-    };
-}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.cpp b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.cpp
deleted file mode 100644
index b687c80a5e84bb10554520b85ce7dcae5b2e5149..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-#include <QPushButton>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "NJointControllerGuiPluginBase.h"
-
-namespace armarx::detail::_NJointControllerGuiPluginBase
-{
-    std::string Base::getControllerName() const
-    {
-        return getName() + "_controller";
-    }
-
-    void Base::createController() {}
-    void Base::activateController() {}
-    void Base::deactivateController() {}
-    void Base::deleteController() {}
-
-    Base::Base() {}
-
-    Base::~Base() {}
-
-    void Base::connectCreateAcivateDeactivateDelete(QPushButton* cr, QPushButton* ac, QPushButton* dc, QPushButton* de)
-    {
-        if (cr)
-        {
-            connect(cr, &QPushButton::clicked, this, &Base::createController);
-        }
-        if (ac)
-        {
-            connect(ac, &QPushButton::clicked, this, &Base::activateController);
-        }
-        if (dc)
-        {
-            connect(dc, &QPushButton::clicked, this, &Base::deactivateController);
-        }
-        if (de)
-        {
-            connect(de, &QPushButton::clicked, this, &Base::deleteController);
-        }
-    }
-
-    void Base::loadSettings(QSettings* settings)
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        getRobotStateComponentPlugin().setRobotStateComponentName(settings->value("rsc", "Armar6StateComponent").toString().toStdString());
-        getRobotUnitComponentPlugin().setRobotUnitName(settings->value("ru", "Armar6Unit").toString().toStdString());
-    }
-
-    void Base::saveSettings(QSettings* settings)
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        settings->setValue("rsc", QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
-        settings->setValue("ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
-    }
-
-    QPointer<QDialog> Base::getConfigDialog(QWidget* parent)
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        if (!_dialog)
-        {
-            _dialog = new SimpleConfigDialog(parent);
-            _dialog->addProxyFinder<RobotUnitInterfacePrx>({"ru", "Robot Unit", "*Unit"});
-            _dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"rsc", "Robot State Component", "*Component"});
-        }
-        return qobject_cast<SimpleConfigDialog*>(_dialog);
-    }
-
-    void Base::configured()
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
-        getRobotUnitComponentPlugin().setRobotUnitName(_dialog->getProxyName("ru"));
-    }
-
-    void Base::preOnConnectComponent()
-    {
-        ARMARX_TRACE;
-        ArmarXComponentWidgetController::preOnConnectComponent();
-        RobotUnitComponentPluginUser::preOnConnectComponent();
-        RobotStateComponentPluginUser::preOnConnectComponent();
-        std::lock_guard g{_allMutex};
-        _robot = addRobot("state robot", VirtualRobot::RobotIO::eStructure);
-    }
-    void Base::postOnConnectComponent()
-    {
-        ARMARX_TRACE;
-        ArmarXComponentWidgetController::postOnConnectComponent();
-        RobotUnitComponentPluginUser::postOnConnectComponent();
-        RobotStateComponentPluginUser::postOnConnectComponent();
-        QMetaObject::invokeMethod(this, "doSetupGuiAfterConnect", Qt::QueuedConnection);
-    }
-    void Base::postOnDisconnectComponent()
-    {
-        ARMARX_TRACE;
-        ArmarXComponentWidgetController::postOnDisconnectComponent();
-        RobotUnitComponentPluginUser::postOnDisconnectComponent();
-        RobotStateComponentPluginUser::postOnDisconnectComponent();
-        std::lock_guard g{_allMutex};
-        deleteController();
-    }
-
-    void Base::doSetupGuiAfterConnect()
-    {
-        ARMARX_TRACE;
-        std::lock_guard g{_allMutex};
-        if (_robot)
-        {
-            synchronizeLocalClone(_robot);
-        }
-        ARMARX_IMPORTANT << "call setupGuiAfterConnect";
-        setupGuiAfterConnect();
-    }
-
-    void Base::setupGuiAfterConnect()
-    {
-
-    }
-}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.h
deleted file mode 100644
index e53b42649a03ca389afb051b3fc6558a23bc32f5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::NJointControllerGuiPluginUtility
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2020
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#pragma once
-
-#include <mutex>
-
-#include <VirtualRobot/Robot.h>
-
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-
-#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-
-
-namespace armarx::detail::_NJointControllerGuiPluginBase
-{
-    class Base :
-        public ArmarXComponentWidgetController,
-        public virtual RobotUnitComponentPluginUser,
-        public virtual RobotStateComponentPluginUser
-    {
-        Q_OBJECT
-    public:
-        virtual NJointControllerConfigPtr readFullCFG() const = 0;
-        std::string getControllerName() const;
-    public Q_SLOTS: //for some reason using slots does not work -> use Q_SLOTS
-        virtual void createController();
-        virtual void activateController();
-        virtual void deactivateController();
-        virtual void deleteController();
-    private Q_SLOTS:
-        void doSetupGuiAfterConnect();
-    public:
-        Base();
-        ~Base() override;
-
-        void connectCreateAcivateDeactivateDelete(QPushButton* cr, QPushButton* ac, QPushButton* dc, QPushButton* de);
-    public:
-        void loadSettings(QSettings* settings) override;
-        void saveSettings(QSettings* settings) override;
-        QPointer<QDialog> getConfigDialog(QWidget* parent) override;
-        void configured() override;
-    public:
-        void onInitComponent() override {}
-        void onConnectComponent() override {}
-        void onDisconnectComponent() override {}
-        void onExitComponent() override {}
-    public:
-        virtual void setupGuiAfterConnect();
-    protected:
-        void preOnConnectComponent() override;
-        void postOnConnectComponent() override;
-        void postOnDisconnectComponent() override;
-    protected:
-        QPointer<SimpleConfigDialog> _dialog;
-
-        mutable std::recursive_mutex     _allMutex;
-        VirtualRobot::RobotPtr          _robot;
-    };
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CMakeLists.txt
deleted file mode 100644
index faaed8324f65c1d74a1cb6d671a6ae0ae57deacc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CMakeLists.txt
+++ /dev/null
@@ -1,17 +0,0 @@
-set(LIB_NAME       RobotAPINJointControllerWidgets)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-set(LIBS
-    RobotUnit
-)
-
-set(SOURCES CartesianImpedanceControllerConfigWidget.cpp)
-set(HEADERS CartesianImpedanceControllerConfigWidget.h)
-set(GUI_MOC_HDRS CartesianImpedanceControllerConfigWidget.h)
-set(GUI_UIS CartesianImpedanceControllerConfigWidget.ui)
-
-if(ArmarXGui_FOUND)
-        armarx_gui_library("${LIB_NAME}" "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${LIBS}")
-endif()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp
deleted file mode 100644
index 800eccc800ea2ad6b8e6e2bb796c95eac73f4d91..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp
+++ /dev/null
@@ -1,238 +0,0 @@
-#include "CartesianImpedanceControllerConfigWidget.h"
-
-namespace armarx
-{
-    void clearLayout(QLayout* layout)
-    {
-        QLayoutItem* item;
-        while ((item = layout->takeAt(0)))
-        {
-            if (item->layout())
-            {
-                clearLayout(item->layout());
-                delete item->layout();
-            }
-            if (item->widget())
-            {
-                delete item->widget();
-            }
-            delete item;
-        }
-    }
-
-    CartesianImpedanceControllerConfigWidget::CartesianImpedanceControllerConfigWidget(QWidget* parent)
-        : QWidget(parent)
-    {
-        ui.setupUi(this);
-
-        kxyz.addWidget(ui.doubleSpinBoxKTX);
-        kxyz.addWidget(ui.doubleSpinBoxKTY);
-        kxyz.addWidget(ui.doubleSpinBoxKTZ);
-        kxyz.setMinMax(0, 5000);
-        kxyz.set(1000);
-
-        krpy.addWidget(ui.doubleSpinBoxKRX);
-        krpy.addWidget(ui.doubleSpinBoxKRY);
-        krpy.addWidget(ui.doubleSpinBoxKRZ);
-        krpy.setMinMax(0, 5000);
-        krpy.set(500);
-
-        dxyz.addWidget(ui.doubleSpinBoxDTX);
-        dxyz.addWidget(ui.doubleSpinBoxDTY);
-        dxyz.addWidget(ui.doubleSpinBoxDTZ);
-        dxyz.setMinMax(0, 5000);
-        dxyz.set(250);
-
-        drpy.addWidget(ui.doubleSpinBoxDRX);
-        drpy.addWidget(ui.doubleSpinBoxDRY);
-        drpy.addWidget(ui.doubleSpinBoxDRZ);
-        drpy.setMinMax(0, 5000);
-        drpy.set(100);
-
-        using T = CartesianImpedanceControllerConfigWidget;
-        connect(ui.pushButtonNullspaceUpdateJoints, &QPushButton::clicked, this, &T::on_pushButtonNullspaceUpdateJoints_clicked);
-        connect(ui.pushButtonNullspaceSend,         &QPushButton::clicked, this, &T::on_pushButtonNullspaceSend_clicked);
-        connect(ui.pushButtonSettingsSend,          &QPushButton::clicked, this, &T::on_pushButtonSettingsSend_clicked);
-    }
-
-    void CartesianImpedanceControllerConfigWidget::loadSettings(QSettings* settings, const QString& prefix)
-    {
-        if (prefix.size())
-        {
-            settings->beginGroup(prefix);
-        }
-        ///TODO
-        if (prefix.size())
-        {
-            settings->endGroup();
-        }
-    }
-
-    void CartesianImpedanceControllerConfigWidget::saveSettings(QSettings* settings, const QString& prefix)
-    {
-        if (prefix.size())
-        {
-            settings->beginGroup("layer");
-        }
-        ///TODO
-        if (prefix.size())
-        {
-            settings->endGroup();
-        }
-    }
-
-    NJointTaskSpaceImpedanceControlConfigPtr
-    CartesianImpedanceControllerConfigWidget::readFullCFG(const Eigen::Vector3f& targPos, const Eigen::Quaternionf& targOri) const
-    {
-        ARMARX_TRACE;
-        ARMARX_CHECK_NOT_NULL(_rns);
-        NJointTaskSpaceImpedanceControlConfigPtr cfg = new NJointTaskSpaceImpedanceControlConfig;
-        cfg->nodeSetName = _rns->getName();
-
-        cfg->desiredPosition = targPos;
-        cfg->desiredOrientation = targOri;
-
-        kxyz.get(cfg->Kpos);
-        krpy.get(cfg->Kori);
-        dxyz.get(cfg->Dpos);
-        drpy.get(cfg->Dori);
-
-        auto [joint, knull, dnull] = readNullspaceCFG();
-        cfg->desiredJointPositions = joint;
-        cfg->Knull = knull;
-        cfg->Dnull = dnull;
-
-        cfg->torqueLimit = ui.doubleSpinBoxTorqueLim->value();
-        return cfg;
-    }
-    NJointTaskSpaceImpedanceControlRuntimeConfig
-    CartesianImpedanceControllerConfigWidget::readRuntimeCFG() const
-    {
-        ARMARX_TRACE;
-        ARMARX_CHECK_NOT_NULL(_rns);
-        NJointTaskSpaceImpedanceControlRuntimeConfig cfg;
-
-        kxyz.get(cfg.Kpos);
-        krpy.get(cfg.Kori);
-        dxyz.get(cfg.Dpos);
-        drpy.get(cfg.Dori);
-
-        auto [joint, knull, dnull] = readNullspaceCFG();
-        cfg.desiredJointPositions = joint;
-        cfg.Knull = knull;
-        cfg.Dnull = dnull;
-
-        cfg.torqueLimit = ui.doubleSpinBoxTorqueLim->value();
-        return cfg;
-    }
-    std::tuple<Eigen::VectorXf, Eigen::VectorXf, Eigen::VectorXf>
-    CartesianImpedanceControllerConfigWidget::readNullspaceCFG() const
-    {
-        ARMARX_TRACE;
-        return
-        {
-            jointValues.get<Eigen::VectorXf>(),
-            jointKnull.get<Eigen::VectorXf>(),
-            jointDnull.get<Eigen::VectorXf>()
-        };
-    }
-
-
-    void CartesianImpedanceControllerConfigWidget::on_pushButtonNullspaceSend_clicked()
-    {
-        ARMARX_TRACE;
-        if (!_controller)
-        {
-            return;
-        }
-        auto [joint, knull, dnull] = readNullspaceCFG();
-        _controller->setNullspaceConfig(joint, knull, dnull);
-    }
-
-    void CartesianImpedanceControllerConfigWidget::on_pushButtonNullspaceUpdateJoints_clicked()
-    {
-        ARMARX_TRACE;
-        if (!_rns)
-        {
-            return;
-        }
-        jointValues.set(_rns);
-    }
-
-    void CartesianImpedanceControllerConfigWidget::on_pushButtonSettingsSend_clicked()
-    {
-        ARMARX_TRACE;
-        if (!_controller)
-        {
-            return;
-        }
-        _controller->setConfig(readRuntimeCFG());
-    }
-
-    void CartesianImpedanceControllerConfigWidget::setController(
-        const NJointTaskSpaceImpedanceControlInterfacePrx& prx
-    )
-    {
-        _controller = prx;
-    }
-    void CartesianImpedanceControllerConfigWidget::setRNS(
-        const VirtualRobot::RobotNodeSetPtr& rns
-    )
-    {
-        ARMARX_TRACE;
-        _rns = rns;
-        auto lay = ui.gridLayoutNullspace;
-        clearLayout(lay);
-        lay->addWidget(new QLabel{"Joint"}, 0, 0);
-        lay->addWidget(new QLabel{"Target"}, 0, 1);
-        lay->addWidget(new QLabel{"Knull"}, 0, 2);
-        lay->addWidget(new QLabel{"Dnull"}, 0, 3);
-        jointValues.clear();
-        jointKnull.clear();
-        jointDnull.clear();
-
-        if (!_rns)
-        {
-            return;
-        }
-
-        int i = 1;
-        for (const auto& rn : _rns->getAllRobotNodes())
-        {
-            ARMARX_TRACE;
-            const auto&& n = rn->getName();
-            lay->addWidget(new QLabel{QString::fromStdString(n)}, i, 0);
-            {
-                auto b = new QDoubleSpinBox;
-                jointValues.addWidget(b);
-                lay->addWidget(b, i, 1);
-                const auto lo = rn->getJointLimitLow();
-                const auto hi = rn->getJointLimitHigh();
-                b->setMinimum(lo);
-                b->setMaximum(hi);
-                b->setValue((lo + hi) / 2);
-            }
-            {
-                auto b = new QDoubleSpinBox;
-                jointKnull.addWidget(b);
-                lay->addWidget(b, i, 2);
-                b->setMinimum(0);
-                b->setMaximum(1000);
-                b->setValue(2);
-            }
-            {
-                auto b = new QDoubleSpinBox;
-                jointDnull.addWidget(b);
-                lay->addWidget(b, i, 3);
-                b->setMinimum(0);
-                b->setMaximum(1000);
-                b->setValue(1);
-            }
-            ++i;
-        }
-        jointKnull.set(2);
-        jointDnull.set(1);
-
-        on_pushButtonNullspaceUpdateJoints_clicked();
-    }
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
deleted file mode 100644
index acf84cb4ede9f5e56d8f278596d0b11c146af5a9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#pragma once
-
-#include <QWidget>
-#include <QSettings>
-
-#include <VirtualRobot/RobotNodeSet.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToPose.h>
-#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
-
-#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/ui_CartesianImpedanceControllerConfigWidget.h>
-
-namespace armarx
-{
-    class CartesianImpedanceControllerConfigWidget : public QWidget
-    {
-    public:
-        CartesianImpedanceControllerConfigWidget(QWidget* parent = nullptr);
-
-        void loadSettings(QSettings* settings, const QString& prefix = "");
-
-        void saveSettings(QSettings* settings, const QString& prefix = "");
-
-        void on_pushButtonNullspaceUpdateJoints_clicked();
-        void on_pushButtonNullspaceSend_clicked();
-        void on_pushButtonSettingsSend_clicked();
-
-        NJointTaskSpaceImpedanceControlConfigPtr
-        readFullCFG(const Eigen::Vector3f& targPos, const Eigen::Quaternionf& targOri) const;
-        NJointTaskSpaceImpedanceControlRuntimeConfig
-        readRuntimeCFG() const;
-        std::tuple<Eigen::VectorXf, Eigen::VectorXf, Eigen::VectorXf>
-        readNullspaceCFG() const;
-
-        ///if null -> send buttons deactivated
-        void setController(const NJointTaskSpaceImpedanceControlInterfacePrx& prx);
-        void setRNS(const VirtualRobot::RobotNodeSetPtr& rns);
-
-        Ui::CartesianImpedanceControllerConfigWidget ui;
-        SpinBoxToVector<QDoubleSpinBox, 3>      kxyz;
-        SpinBoxToVector<QDoubleSpinBox, 3>      krpy;
-        SpinBoxToVector<QDoubleSpinBox, 3>      dxyz;
-        SpinBoxToVector<QDoubleSpinBox, 3>      drpy;
-        SpinBoxToVector<QDoubleSpinBox>         jointValues;
-        SpinBoxToVector<QDoubleSpinBox>         jointKnull;
-        SpinBoxToVector<QDoubleSpinBox>         jointDnull;
-
-    private:
-        NJointTaskSpaceImpedanceControlInterfacePrx _controller;
-        VirtualRobot::RobotNodeSetPtr               _rns;
-    };
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui
deleted file mode 100644
index 669892bd956030629ddc31abc71804c4c8e31140..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui
+++ /dev/null
@@ -1,272 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>CartesianImpedanceControllerConfigWidget</class>
- <widget class="QWidget" name="CartesianImpedanceControllerConfigWidget">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>540</width>
-    <height>303</height>
-   </rect>
-  </property>
-  <property name="windowTitle">
-   <string>CartesianImpedanceControllerWidget</string>
-  </property>
-  <layout class="QGridLayout" name="gridLayout">
-   <property name="leftMargin">
-    <number>0</number>
-   </property>
-   <property name="topMargin">
-    <number>0</number>
-   </property>
-   <property name="rightMargin">
-    <number>0</number>
-   </property>
-   <property name="bottomMargin">
-    <number>0</number>
-   </property>
-   <item row="0" column="0">
-    <widget class="QWidget" name="widget" native="true">
-     <layout class="QGridLayout" name="gridLayout_3">
-      <item row="2" column="1">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxKRX">
-        <property name="toolTip">
-         <string>200 - 800 (use 500)</string>
-        </property>
-       </widget>
-      </item>
-      <item row="4" column="3">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxDRZ">
-        <property name="toolTip">
-         <string>50 - 500</string>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="0">
-       <widget class="QLabel" name="label_5">
-        <property name="toolTip">
-         <string>200 - 800 (use 500)</string>
-        </property>
-        <property name="text">
-         <string>K RPY</string>
-        </property>
-       </widget>
-      </item>
-      <item row="5" column="1" colspan="3">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxTorqueLim">
-        <property name="maximum">
-         <double>1000.000000000000000</double>
-        </property>
-        <property name="value">
-         <double>20.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="1">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxKTX">
-        <property name="toolTip">
-         <string>500 - 2000 (use 1000)</string>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="0">
-       <widget class="QLabel" name="label_3">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="toolTip">
-         <string>500 - 2000 (use 1000)</string>
-        </property>
-        <property name="text">
-         <string>K XYZ</string>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="2">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxKRY">
-        <property name="toolTip">
-         <string>200 - 800 (use 500)</string>
-        </property>
-       </widget>
-      </item>
-      <item row="5" column="0">
-       <widget class="QLabel" name="label_6">
-        <property name="text">
-         <string>Torque Limit</string>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="2">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxKTY">
-        <property name="toolTip">
-         <string>500 - 2000 (use 1000)</string>
-        </property>
-       </widget>
-      </item>
-      <item row="4" column="2">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxDRY">
-        <property name="toolTip">
-         <string>50 - 500</string>
-        </property>
-       </widget>
-      </item>
-      <item row="4" column="1">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxDRX">
-        <property name="toolTip">
-         <string>50 - 500</string>
-        </property>
-       </widget>
-      </item>
-      <item row="3" column="1">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxDTX">
-        <property name="toolTip">
-         <string>100 - 500</string>
-        </property>
-       </widget>
-      </item>
-      <item row="0" column="0" colspan="4">
-       <widget class="QGroupBox" name="groupBox_3">
-        <property name="title">
-         <string>Nullspace</string>
-        </property>
-        <layout class="QVBoxLayout" name="verticalLayout_4">
-         <property name="leftMargin">
-          <number>0</number>
-         </property>
-         <property name="topMargin">
-          <number>0</number>
-         </property>
-         <property name="rightMargin">
-          <number>0</number>
-         </property>
-         <property name="bottomMargin">
-          <number>0</number>
-         </property>
-         <item>
-          <widget class="QWidget" name="widget_3" native="true">
-           <layout class="QGridLayout" name="gridLayout_4">
-            <item row="0" column="0" colspan="2">
-             <layout class="QGridLayout" name="gridLayoutNullspace">
-              <item row="0" column="1">
-               <widget class="QLabel" name="label_11">
-                <property name="text">
-                 <string>Pos</string>
-                </property>
-               </widget>
-              </item>
-              <item row="0" column="0">
-               <widget class="QLabel" name="label_9">
-                <property name="text">
-                 <string>Joint</string>
-                </property>
-               </widget>
-              </item>
-              <item row="0" column="3">
-               <widget class="QLabel" name="label_10">
-                <property name="toolTip">
-                 <string>0.5 - 1</string>
-                </property>
-                <property name="text">
-                 <string>Dnull</string>
-                </property>
-               </widget>
-              </item>
-              <item row="0" column="2">
-               <widget class="QLabel" name="label_12">
-                <property name="toolTip">
-                 <string>1 - 10</string>
-                </property>
-                <property name="text">
-                 <string>Knull</string>
-                </property>
-               </widget>
-              </item>
-             </layout>
-            </item>
-            <item row="1" column="0">
-             <widget class="QPushButton" name="pushButtonNullspaceSend">
-              <property name="text">
-               <string>Send</string>
-              </property>
-             </widget>
-            </item>
-            <item row="1" column="1">
-             <widget class="QPushButton" name="pushButtonNullspaceUpdateJoints">
-              <property name="text">
-               <string>Set to current</string>
-              </property>
-             </widget>
-            </item>
-           </layout>
-          </widget>
-         </item>
-        </layout>
-       </widget>
-      </item>
-      <item row="3" column="3">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxDTZ">
-        <property name="toolTip">
-         <string>100 - 500</string>
-        </property>
-       </widget>
-      </item>
-      <item row="4" column="0">
-       <widget class="QLabel" name="label_2">
-        <property name="toolTip">
-         <string>50 - 500</string>
-        </property>
-        <property name="text">
-         <string>D RPY</string>
-        </property>
-       </widget>
-      </item>
-      <item row="3" column="2">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxDTY">
-        <property name="toolTip">
-         <string>100 - 500</string>
-        </property>
-       </widget>
-      </item>
-      <item row="3" column="0">
-       <widget class="QLabel" name="label_4">
-        <property name="toolTip">
-         <string>100 - 500</string>
-        </property>
-        <property name="text">
-         <string>D XYZ</string>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="3">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxKRZ">
-        <property name="toolTip">
-         <string>200 - 800 (use 500)</string>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="3">
-       <widget class="QDoubleSpinBox" name="doubleSpinBoxKTZ">
-        <property name="toolTip">
-         <string>500 - 2000 (use 1000)</string>
-        </property>
-       </widget>
-      </item>
-      <item row="6" column="0" colspan="4">
-       <widget class="QPushButton" name="pushButtonSettingsSend">
-        <property name="text">
-         <string>Send</string>
-        </property>
-       </widget>
-      </item>
-     </layout>
-    </widget>
-   </item>
-  </layout>
- </widget>
- <resources/>
- <connections/>
-</ui>
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/CMakeLists.txt
deleted file mode 100644
index 8297acb5533fdef80475b6123029aca227168354..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/CMakeLists.txt
+++ /dev/null
@@ -1,65 +0,0 @@
-set(LIB_NAME       BimanualForceControllers)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-
-find_package(Eigen3 QUIET)
-find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
-find_package(DMP QUIET)
-
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-armarx_build_if(DMP_FOUND "DMP not available")
-
-if (Eigen3_FOUND AND Simox_FOUND AND DMP_FOUND)
-    include_directories(${Simox_INCLUDE_DIRS})
-    include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
-    include_directories(${DMP_INCLUDE_DIRS})
-
-endif()
-
-
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-#
-# all include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-#    include_directories(${MyLib_INCLUDE_DIRS})
-#endif()
-
-set(LIBS
-	ArmarXCoreInterfaces 
-	ArmarXCore
-        RobotAPIInterfaces
-        RobotAPICore
-        RobotAPIUnits
-        ArmarXCoreObservers
-        ArmarXCoreStatechart
-        ArmarXCoreEigen3Variants
-        VirtualRobot
-        Saba
-        SimDynamics
-        RobotUnit
-        BimanualForceControlInterfaces
-        ${DMP_LIBRARIES}
-        DMPController
-#/common/homes/students/jianfeng/armarx/RobotAPI/build/lib/libDMPController.so.0.9.2
- )
-
-set(LIB_FILES
-./NJointBimanualForceController.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(LIB_HEADERS
-./NJointBimanualForceController.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
-
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-# add unit tests
-#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp
deleted file mode 100644
index 2e61a616541c4fad51225d6f363dd9dfa4d20a7a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp
+++ /dev/null
@@ -1,884 +0,0 @@
-#include <SimoxUtility/math/pose/pose.h>
-#include <SimoxUtility/math/pose/is_homogeneous_transform.h>
-#include <SimoxUtility/math/isfinite.h>
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include "NJointBimanualCartesianAdmittanceController.h"
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointBimanualCartesianAdmittanceController> registrationControllerNJointBimanualCartesianAdmittanceController("NJointBimanualCartesianAdmittanceController");
-
-    NJointBimanualCartesianAdmittanceController::NJointBimanualCartesianAdmittanceController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "Preparing ... bimanual ";
-        useSynchronizedRtRobot();
-        auto cfgPtr = NJointBimanualCartesianAdmittanceControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_NOT_NULL(cfgPtr);
-        //init rtData
-        {
-            auto initRtData = [&](auto & data, const auto & rnsName, const auto & ftName, const auto & ftRefFrame)
-            {
-                data.rns = rtGetRobot()->getRobotNodeSet(rnsName);
-                ARMARX_CHECK_NOT_NULL(data.rns) << "No robot node set " << rnsName;
-                data.tcp = data.rns->getTCP();
-                ARMARX_CHECK_NOT_NULL(data.tcp) << "No TCP in robot node set " << rnsName;
-                data.frameFTSensor = rtGetRobot()->getRobotNode(ftRefFrame);
-                ARMARX_CHECK_NOT_NULL(data.frameFTSensor) << "No ref frame for ft sensor in robot " << ftRefFrame;
-
-                for (size_t i = 0; i < data.rns->getSize(); ++i)
-                {
-                    std::string jointName = data.rns->getNode(i)->getName();
-                    data.jointNames.push_back(jointName);
-                    ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-                    const SensorValueBase* sv = useSensorValue(jointName);
-                    ARMARX_CHECK_NOT_NULL(ct) << "No control target available for " << jointName;
-                    ARMARX_CHECK_NOT_NULL(sv) << "No sensor value available for " << jointName;
-                    data.targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-                    const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-                    const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-                    ARMARX_CHECK_NOT_NULL(velocitySensor) << "No velocitySensor available for " << jointName;
-                    ARMARX_CHECK_NOT_NULL(positionSensor) << "No positionSensor available for " << jointName;
-                    data.velocitySensors.push_back(velocitySensor);
-                    data.positionSensors.push_back(positionSensor);
-                }
-                const auto ftDev = robUnit->getSensorDevice(ftName);
-                ARMARX_CHECK_NOT_NULL(ftDev) << "No sensor device available for " << ftName;
-                const SensorValueBase* svlf = ftDev->getSensorValue();
-                ARMARX_CHECK_NOT_NULL(svlf) << "No sensor value available for " << ftName;
-                data.forceTorque = svlf->asA<SensorValueForceTorque>();
-                ARMARX_CHECK_NOT_NULL(data.forceTorque) << "Sensor value for " << ftName << " is not of type SensorValueForceTorque";
-                data.IK.reset(new VirtualRobot::DifferentialIK(
-                                  data.rns,
-                                  data.rns->getRobot()->getRootNode(),
-                                  VirtualRobot::JacobiProvider::eSVDDamped));
-            };
-
-            initRtData(rt.left, cfgPtr->kinematicChainLeft, cfgPtr->ftSensorLeft, cfgPtr->ftSensorLeftFrame);
-            initRtData(rt.right, cfgPtr->kinematicChainRight, cfgPtr->ftSensorRight, cfgPtr->ftSensorRightFrame);
-        }
-
-        //init cfg + check it
-        {
-            setConfig(cfgPtr);
-            cfgBuf.reinitAllBuffers(cfgBuf.getWriteBuffer());
-        }
-
-
-        //        {
-        //            rt2ControlData initSensorData;
-        //            initSensorData.deltaT = 0;
-        //            initSensorData.currentTime = 0;
-        //            initSensorData.currentPose = boxInitialPose;
-        //            initSensorData.currentTwist.setZero();
-        //            rt2ControlBuffer.reinitAllBuffers(initSensorData);
-        //        }
-
-
-        //        {
-        //            ControlInterfaceData initInterfaceData;
-        //            initInterfaceData.currentLeftPose = rt.left.tcp->getPoseInRootFrame();
-        //            initInterfaceData.currentRightPose = rt.right.tcp->getPoseInRootFrame();
-        //            controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
-        //        }
-
-        //////////////////////////////TODO
-        //        leftInitialPose = rt.left.tcp->getPoseInRootFrame();
-        //        rightInitialPose = rt.right.rns->getPoseInRootFrame();
-        //        leftInitialPose.block<3, 1>(0, 3) = 0.001 * leftInitialPose.block<3, 1>(0, 3);
-        //        rightInitialPose.block<3, 1>(0, 3) = 0.001 * rightInitialPose.block<3, 1>(0, 3);
-
-        //        //        leftInitialPose = boxInitialPose;
-        //        //        leftInitialPose(0, 3) -= cfg->boxWidth * 0.5;
-        //        //        rightInitialPose = boxInitialPose;
-        //        //        rightInitialPose(0, 3) += cfg->boxWidth * 0.5;
-        //////////////////////////////TODO
-        //        forcePIDControllers.resize(12);
-        //        for (size_t i = 0; i < 6; i++)
-        //        {
-        //            forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
-        //            forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
-        //            forcePIDControllers.at(i)->reset();
-        //            forcePIDControllers.at(i + 6)->reset();
-        //        }
-        //////////////////////////////TODO
-        // filter
-        //        filterCoeff = cfg->filterCoeff;
-        //        ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
-        //        filteredOldValue.setZero(12);
-
-        // static compensation
-        rt.left.sensorFrame2TcpFrame.setZero();
-        rt.right.sensorFrame2TcpFrame.setZero();
-        //        NJointBimanualObjLevelControlData initData;
-        //        initData.boxPose = boxInitialPose;
-        //        initData.boxTwist.setZero(6);
-        //        reinitTripleBuffer(initData);
-
-        //        ARMARX_INFO << "left initial pose: \n" << leftInitialPose  << "\n right initial pose: \n" << rightInitialPose;
-
-        //        ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
-        ARMARX_IMPORTANT << "finished construction!";
-
-        //        targetWrench.setZero(cfg->targetWrench.size());
-        //        for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
-        //        {
-        //            targetWrench(i) = cfg->targetWrench[i];
-        //        }
-    }
-
-
-    void NJointBimanualCartesianAdmittanceController::rtPreActivateController()
-    {
-        //        NJointBimanualObjLevelControlData initData;
-        //        initData.boxPose = boxInitPose;
-        //        initData.boxTwist.resize(6);
-        //        reinitTripleBuffer(initData);
-
-        rt.virtualAcc.setZero();
-        rt.virtualVel.setZero();
-        rt.virtualPose.setZero();
-        rt.filteredOldValue.setZero();
-        //        rt.ftOffset.setZero();
-        rt.firstLoop = true;
-        rt.ftcalibrationTimer = 0;
-
-
-
-        const Eigen::Matrix4f leftPose = simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
-        const Eigen::Matrix4f rightPose = simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
-
-        rt.virtualPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
-        rt.virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
-
-        const Eigen::Matrix4f leftSensorFrame = simox::math::scaled_position(
-                rt.left.frameFTSensor->getPoseInRootFrame(), 0.001);
-        const Eigen::Matrix4f rightSensorFrame = simox::math::scaled_position(
-                    rt.right.frameFTSensor->getPoseInRootFrame(), 0.001);
-
-        rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
-        rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
-
-        //        ARMARX_INFO << "modified left pose:\n " << leftPose;
-        //        ARMARX_INFO << "modified right pose:\n " << rightPose;
-    }
-
-    std::string NJointBimanualCartesianAdmittanceController::getClassName(const Ice::Current&) const
-    {
-        return "NJointBimanualCartesianAdmittanceController";
-    }
-
-    //    void NJointBimanualCartesianAdmittanceController::controllerRun()
-    //    {
-    //        if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
-    //        {
-    //            return;
-    //        }
-
-    //        double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
-    //        Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
-    //        Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
-    //        //ARMARX_IMPORTANT << "canVal:  " << objectDMP->canVal;
-
-    //        if (objectDMP->canVal < 1e-8)
-    //        {
-    //            finished = true;
-    //            dmpStarted = false;
-    //        }
-
-    //        objectDMP->flow(deltaT, currentPose, currentTwist);
-
-    //        LockGuardType guard {controlDataMutex};
-    //        getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
-    //        getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
-    //        writeControlStruct();
-    //    }
-
-
-
-
-    void NJointBimanualCartesianAdmittanceController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
-    {
-
-        const Eigen::Matrix4f currentLeftPose = simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
-        const Eigen::Matrix4f currentRightPose = simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
-        const Eigen::Matrix4f currentBoxPose = [&]
-        {
-            Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
-            pose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
-            pose.block<3, 3>(0, 0) = currentLeftPose.block<3, 3>(0, 0);
-            return pose;
-        }();
-        //        {
-        //            controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
-        //            controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
-        //            controlInterfaceBuffer.commitWrite();
-        //        }
-        const double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-        ARMARX_ON_SCOPE_EXIT{rt.firstLoop = false;};
-
-        rt.ftcalibrationTimer += deltaT;
-
-        // -------------------------------------------- config ---------------------------------------------
-
-        if (cfgBuf.updateReadBuffer())
-        {
-            auto& cfg = cfgBuf._getNonConstReadBuffer();
-            const Eigen::Vector3f tmpL = rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecLeft;
-            const Eigen::Vector3f tmpR = rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecRight;
-            cfg.CoMVecLeft = tmpL;
-            cfg.CoMVecRight = tmpR;
-        }
-        if (rt.firstLoop)
-        {
-            auto& trg = targBuf._getNonConstReadBuffer();
-            trg.pose = currentBoxPose;
-            trg.vel.setZero();
-        }
-        auto& dbgOut = debugOutputData.getWriteBuffer();
-        const auto& targ = targBuf.getWriteBuffer();
-        const auto& cfg = cfgBuf.getReadBuffer();
-
-        if (rt.ftcalibrationTimer < cfg.ftCalibrationTime)
-        {
-            //            rt.ftOffset.block<3, 1>(0, 0) = 0.5 * rt.ftOffset.block<3, 1>(0, 0) + 0.5 * rt.right.forceTorque->force;
-            //            rt.ftOffset.block<3, 1>(3, 0) = 0.5 * rt.ftOffset.block<3, 1>(3, 0) + 0.5 * rt.right.forceTorque->torque;
-            //            rt.ftOffset.block<3, 1>(6, 0) = 0.5 * rt.ftOffset.block<3, 1>(6, 0) + 0.5 * rt.left.forceTorque->force;
-            //            rt.ftOffset.block<3, 1>(9, 0) = 0.5 * rt.ftOffset.block<3, 1>(9, 0) + 0.5 * rt.left.forceTorque->torque;
-            //            cfg.KmAdmittance.setZero();
-        }
-
-        const Eigen::Vector6f KmAdmittance =
-            (rt.ftcalibrationTimer < cfg.ftCalibrationTime) ?
-            Eigen::Vector6f::Zero() :
-            cfg.KmAdmittance;
-
-        // -------------------------------------------- target wrench ---------------------------------------------
-        const Eigen::Vector12f deltaPoseForWrenchControl = cfg.targetWrench.array() / cfg.KpImpedance.array();
-
-        // ------------------------------------------- current tcp pose -------------------------------------------
-
-
-        // --------------------------------------------- grasp matrix ---------------------------------------------
-        const auto skew = [](auto & vec)
-        {
-            Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
-            mat(1, 2) = -vec(0);
-            mat(0, 2) = vec(1);
-            mat(0, 1) = -vec(2);
-            mat(2, 1) = vec(0);
-            mat(2, 0) = -vec(1);
-            mat(1, 0) = vec(2);
-            return mat;
-        };
-        const Eigen::Vector3f objCom2TCPLeft{-cfg.boxWidth * 0.5f, 0.f, 0.f};
-        const Eigen::Vector3f objCom2TCPRight{+cfg.boxWidth * 0.5f, 0.f, 0.f};
-
-        Eigen::Matrix_6_12_f graspMatrix;
-        graspMatrix.setZero();
-        graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
-        graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
-
-        const Eigen::Vector3f rLeft = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPLeft;
-        const Eigen::Vector3f rRight = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPRight;
-
-        graspMatrix.block<3, 3>(3, 0) = skew(rLeft);
-        graspMatrix.block<3, 3>(3, 6) = skew(rRight);
-
-        // // projection of grasp matrix
-        // Eigen::MatrixXf pinvG = rt.left.IK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
-        // Eigen::MatrixXf G_range = pinvG * graspMatrix;
-        // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
-        float lambda = 1;
-        const Eigen::MatrixXf pinvGraspMatrixT = rt.left.IK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
-
-        // ---------------------------------------------- object pose ----------------------------------------------
-        Eigen::Matrix4f boxCurrentPose = currentRightPose;
-        boxCurrentPose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
-        Eigen::Vector6f boxCurrentTwist = Eigen::Vector6f::Zero();
-
-        // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
-        const Eigen::MatrixXf I = Eigen::MatrixXf::Identity(rt.left.targets.size(), rt.left.targets.size());
-        // Jacobian matrices
-        Eigen::MatrixXf jacobiL = rt.left.IK->getJacobianMatrix(rt.left.tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-        Eigen::MatrixXf jacobiR = rt.right.IK->getJacobianMatrix(rt.right.tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-        jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
-        jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
-        // qpos, qvel
-        Eigen::VectorXf leftqpos(rt.left.positionSensors.size());
-        Eigen::VectorXf leftqvel(rt.left.velocitySensors.size());
-        for (size_t i = 0; i < rt.left.velocitySensors.size(); ++i)
-        {
-            leftqpos(i) = rt.left.positionSensors[i]->position;
-            leftqvel(i) = rt.left.velocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf rightqpos(rt.right.positionSensors.size());
-        Eigen::VectorXf rightqvel(rt.right.velocitySensors.size());
-        for (size_t i = 0; i < rt.right.velocitySensors.size(); ++i)
-        {
-            rightqpos(i) = rt.right.positionSensors[i]->position;
-            rightqvel(i) = rt.right.velocitySensors[i]->velocity;
-        }
-
-        // -------------------------------------- compute TCP and object velocity -------------------------------------
-        const Eigen::Vector6f currentLeftTwist = jacobiL * leftqvel;
-        const Eigen::Vector6f currentRightTwist = jacobiR * rightqvel;
-
-        Eigen::Vector12f currentTwist;
-        currentTwist << currentLeftTwist, currentRightTwist;
-        boxCurrentTwist = pinvGraspMatrixT * currentTwist;
-
-        //        rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
-        //        rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
-        //        rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
-        //        rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
-        //        rt2ControlBuffer.commitWrite();
-
-        // --------------------------------------------- get ft sensor ---------------------------------------------
-        // static compensation
-        const Eigen::Vector3f gravity{0.0, 0.0, -9.8};
-        const Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
-        const Eigen::Vector3f localForceVecLeft = cfg.massLeft * localGravityLeft;
-        const Eigen::Vector3f localTorqueVecLeft = cfg.CoMVecLeft.cross(localForceVecLeft);
-
-        const Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
-        const Eigen::Vector3f localForceVecRight = cfg.massRight * localGravityRight;
-        const Eigen::Vector3f localTorqueVecRight = cfg.CoMVecRight.cross(localForceVecRight);
-
-        // mapping of measured wrenches
-        Eigen::Vector12f wrenchesMeasured;
-        wrenchesMeasured << rt.right.forceTorque->force - cfg.forceOffsetLeft,
-                         rt.right.forceTorque->torque - cfg.torqueOffsetLeft,
-                         rt.left.forceTorque->force - cfg.forceOffsetRight,
-                         rt.left.forceTorque->torque - cfg.torqueOffsetRight;
-        for (size_t i = 0; i < 12; i++)
-        {
-            wrenchesMeasured(i) = (1 - cfg.filterCoeff) * wrenchesMeasured(i) + cfg.filterCoeff * rt.filteredOldValue(i);
-        }
-        rt.filteredOldValue = wrenchesMeasured;
-        wrenchesMeasured.block<3, 1>(0, 0) = rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
-        wrenchesMeasured.block<3, 1>(3, 0) = rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
-        wrenchesMeasured.block<3, 1>(6, 0) = rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
-        wrenchesMeasured.block<3, 1>(9, 0) = rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
-        //        if (wrenchesMeasured.norm() < cfg->forceThreshold)
-        //        {
-        //            wrenchesMeasured.setZero();
-        //        }
-
-        Eigen::Vector12f wrenchesMeasuredInRoot;
-        wrenchesMeasuredInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
-        wrenchesMeasuredInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
-        wrenchesMeasuredInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
-        wrenchesMeasuredInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
-
-        // map to object
-        Eigen::Vector6f objFTValue = graspMatrix * wrenchesMeasuredInRoot;
-        for (size_t i = 0; i < 6; i++)
-        {
-            if (fabs(objFTValue(i)) < cfg.forceThreshold(i))
-            {
-                objFTValue(i) = 0;
-            }
-            else
-            {
-                objFTValue(i) -= cfg.forceThreshold(i) * objFTValue(i) / fabs(objFTValue(i));
-            }
-        }
-
-        // --------------------------------------------- get MP target ---------------------------------------------
-        const Eigen::Matrix4f boxPose = targ.pose;
-        const Eigen::Vector6f boxTwist = targ.vel;
-        // --------------------------------------------- obj admittance control ---------------------------------------------
-        // admittance
-        Eigen::Vector6f objPoseError;
-        objPoseError.head(3) = rt.virtualPose.block<3, 1>(0, 3) - boxPose.block<3, 1>(0, 3);
-        const Eigen::Matrix3f objDiffMat = rt.virtualPose.block<3, 3>(0, 0) * boxPose.block<3, 3>(0, 0).transpose();
-        objPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
-
-
-        Eigen::Vector6f objAcc = Eigen::Vector6f::Zero();
-        Eigen::Vector6f objVel = Eigen::Vector6f::Zero();
-        for (size_t i = 0; i < 6; i++)
-        {
-            objAcc(i) = KmAdmittance(i) * objFTValue(i)
-                        - cfg.KpAdmittance(i) * objPoseError(i)
-                        - cfg.KdAdmittance(i) * rt.virtualVel(i);
-        }
-        objVel = rt.virtualVel + 0.5 * deltaT * (objAcc + rt.virtualAcc);
-        const Eigen::Vector6f deltaObjPose = 0.5 * deltaT * (objVel + rt.virtualVel);
-        rt.virtualAcc = objAcc;
-        rt.virtualVel = objVel;
-        rt.virtualPose.block<3, 1>(0, 3) += deltaObjPose.head(3);
-        rt.virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(
-                                               deltaObjPose(3),
-                                               deltaObjPose(4),
-                                               deltaObjPose(5)) * rt.virtualPose.block<3, 3>(0, 0);
-
-        // --------------------------------------------- convert to tcp pose ---------------------------------------------
-        Eigen::Matrix4f tcpTargetPoseLeft = rt.virtualPose;
-        Eigen::Matrix4f tcpTargetPoseRight = rt.virtualPose;
-        tcpTargetPoseLeft.block<3, 1>(0, 3) += rt.virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeft - deltaPoseForWrenchControl.block<3, 1>(0, 0));
-        tcpTargetPoseRight.block<3, 1>(0, 3) += rt.virtualPose.block<3, 3>(0, 0) * (objCom2TCPRight - deltaPoseForWrenchControl.block<3, 1>(6, 0));
-
-        // --------------------------------------------- Impedance control ---------------------------------------------
-        Eigen::Vector12f poseError;
-        Eigen::Matrix3f diffMat = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(0, 0) = tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
-        poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        diffMat = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(6, 0) = tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
-        poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        Eigen::Vector12f forceImpedance;
-        for (size_t i = 0; i < 12; i++)
-        {
-            forceImpedance(i) = cfg.KpImpedance(i) * poseError(i) - cfg.KdImpedance(i) * currentTwist(i);
-            //            forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
-        }
-
-        // --------------------------------------------- Nullspace control ---------------------------------------------
-        const Eigen::VectorXf leftNullspaceTorque  = cfg.knull * (cfg.desiredJointValuesLeft  - leftqpos) - cfg.dnull * leftqvel;
-        const Eigen::VectorXf rightNullspaceTorque = cfg.knull * (cfg.desiredJointValuesRight - rightqpos) - cfg.dnull * rightqvel;
-
-        // --------------------------------------------- Set Torque Control Command ---------------------------------------------
-        //        float lambda = 1;
-
-        // torque limit
-        const auto setTargets = [&](auto & rtarm, const auto & jacobi, const auto & nullspaceTorque, int forceImpOffset)
-        {
-            const Eigen::MatrixXf jtpinv = rtarm.IK->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
-            const Eigen::VectorXf desiredJointTorques = jacobi.transpose() * forceImpedance.block<6, 1>(forceImpOffset, 0) +
-                    (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-            for (size_t i = 0; i < rtarm.targets.size(); ++i)
-            {
-                float desiredTorque =   desiredJointTorques(i);
-                if (isnan(desiredTorque))
-                {
-                    desiredTorque = 0;
-                }
-                desiredTorque = (desiredTorque >  cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
-                desiredTorque = (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
-                dbgOut.desired_torques[rtarm.jointNames[i]] = desiredJointTorques(i);
-                rtarm.targets.at(i)->torque = desiredTorque;
-            }
-        };
-        setTargets(rt.left,  jacobiL, leftNullspaceTorque, 0);
-        setTargets(rt.right, jacobiR, rightNullspaceTorque, 6);
-        {
-            const Eigen::MatrixXf jtpinvL = rt.left.IK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
-            const Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceImpedance.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
-            for (size_t i = 0; i < rt.left.targets.size(); ++i)
-            {
-                float desiredTorque =   leftJointDesiredTorques(i);
-                if (isnan(desiredTorque))
-                {
-                    desiredTorque = 0;
-                }
-                desiredTorque = (desiredTorque >  cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
-                desiredTorque = (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
-                dbgOut.desired_torques[rt.left.jointNames[i]] = leftJointDesiredTorques(i);
-                rt.left.targets.at(i)->torque = desiredTorque;
-            }
-        }
-
-        {
-            const Eigen::MatrixXf jtpinvR = rt.right.IK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
-            const Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceImpedance.tail(6) + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-            for (size_t i = 0; i < rt.right.targets.size(); ++i)
-            {
-                float desiredTorque = rightJointDesiredTorques(i);
-                if (isnan(desiredTorque))
-                {
-                    desiredTorque = 0;
-                }
-                desiredTorque = (desiredTorque >   cfg.torqueLimit) ?  cfg.torqueLimit : desiredTorque;
-                desiredTorque = (desiredTorque < - cfg.torqueLimit) ? - cfg.torqueLimit : desiredTorque;
-                dbgOut.desired_torques[rt.right.jointNames[i]] = rightJointDesiredTorques(i);
-                rt.right.targets.at(i)->torque = desiredTorque;
-            }
-        }
-
-        // --------------------------------------------- debug output ---------------------------------------------
-        dbgOut.currentBoxPose = currentBoxPose;
-        dbgOut.forceImpedance = forceImpedance;
-        dbgOut.poseError = poseError;
-        //        dbgOut.wrenchesConstrained = wrenchesConstrained;
-        dbgOut.wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
-        //        dbgOut.wrenchDMP = wrenchDMP;
-        //        dbgOut.computedBoxWrench = computedBoxWrench;
-
-        dbgOut.virtualPose_x = rt.virtualPose(0, 3);
-        dbgOut.virtualPose_y = rt.virtualPose(1, 3);
-        dbgOut.virtualPose_z = rt.virtualPose(2, 3);
-
-        dbgOut.objPose_x = boxCurrentPose(0, 3);
-        dbgOut.objPose_y = boxCurrentPose(1, 3);
-        dbgOut.objPose_z = boxCurrentPose(2, 3);
-
-        dbgOut.objForce_x = objFTValue(0);
-        dbgOut.objForce_y = objFTValue(1);
-        dbgOut.objForce_z = objFTValue(2);
-        dbgOut.objTorque_x = objFTValue(3);
-        dbgOut.objTorque_y = objFTValue(4);
-        dbgOut.objTorque_z = objFTValue(5);
-
-        dbgOut.objVel_x = objVel(0);
-        dbgOut.objVel_y = objVel(1);
-        dbgOut.objVel_z = objVel(2);
-        dbgOut.objVel_rx = objVel(3);
-        dbgOut.objVel_ry = objVel(4);
-        dbgOut.objVel_rz = objVel(5);
-
-        dbgOut.deltaPose_x = deltaObjPose(0);
-        dbgOut.deltaPose_y = deltaObjPose(1);
-        dbgOut.deltaPose_z = deltaObjPose(2);
-        dbgOut.deltaPose_rx = deltaObjPose(3);
-        dbgOut.deltaPose_ry = deltaObjPose(4);
-        dbgOut.deltaPose_rz = deltaObjPose(5);
-
-        dbgOut.modifiedPoseRight_x = tcpTargetPoseRight(0, 3);
-        dbgOut.modifiedPoseRight_y = tcpTargetPoseRight(1, 3);
-        dbgOut.modifiedPoseRight_z = tcpTargetPoseRight(2, 3);
-
-        dbgOut.currentPoseLeft_x = currentLeftPose(0, 3);
-        dbgOut.currentPoseLeft_y = currentLeftPose(1, 3);
-        dbgOut.currentPoseLeft_z = currentLeftPose(2, 3);
-
-
-
-        dbgOut.modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
-        dbgOut.modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
-        dbgOut.modifiedPoseLeft_z = tcpTargetPoseLeft(2, 3);
-
-        dbgOut.currentPoseRight_x = currentRightPose(0, 3);
-        dbgOut.currentPoseRight_y = currentRightPose(1, 3);
-        dbgOut.currentPoseRight_z = currentRightPose(2, 3);
-
-
-        dbgOut.dmpBoxPose_x = boxPose(0, 3);
-        dbgOut.dmpBoxPose_y = boxPose(1, 3);
-        dbgOut.dmpBoxPose_z = boxPose(2, 3);
-
-        dbgOut.dmpTwist_x = boxTwist(0);
-        dbgOut.dmpTwist_y = boxTwist(1);
-        dbgOut.dmpTwist_z = boxTwist(2);
-        dbgOut.rx = rRight(0);
-        dbgOut.ry = rRight(1);
-        dbgOut.rz = rRight(2);
-
-        //        dbgOut.modifiedTwist_lx = twistDMP(0);
-        //        dbgOut.modifiedTwist_ly = twistDMP(1);
-        //        dbgOut.modifiedTwist_lz = twistDMP(2);
-        //        dbgOut.modifiedTwist_rx = twistDMP(6);
-        //        dbgOut.modifiedTwist_ry = twistDMP(7);
-        //        dbgOut.modifiedTwist_rz = twistDMP(8);
-
-        //        dbgOut.forcePID = forcePIDInRootForDebug;
-
-        debugOutputData.commitWrite();
-
-    }
-
-    void NJointBimanualCartesianAdmittanceController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-        std::lock_guard guard{debugOutputDataReadMutex};
-        const auto& buf = debugOutputData.getUpToDateReadBuffer();
-        StringVariantBaseMap datafields;
-
-        for (const auto& [name, val] : buf.desired_torques)
-        {
-            datafields[name] = new Variant(val);
-        }
-
-        const auto& reportElements = [&](const auto & vec, const std::string & pre)
-        {
-            for (int i = 0; i < vec.rows(); ++i)
-            {
-                datafields[pre + std::to_string(i)] = new Variant(vec(i));
-            }
-        };
-        reportElements(buf.forceImpedance, "forceImpedance_");
-        reportElements(buf.forcePID, "forcePID_");
-        reportElements(buf.poseError, "poseError_");
-        reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
-        reportElements(buf.wrenchesMeasuredInRoot, "wrenchesMeasuredInRoot_");
-        reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
-        reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
-
-        datafields["virtualPose_x"] = new Variant(buf.virtualPose_x);
-        datafields["virtualPose_y"] = new Variant(buf.virtualPose_y);
-        datafields["virtualPose_z"] = new Variant(buf.virtualPose_z);
-
-        datafields["objPose_x"] = new Variant(buf.objPose_x);
-        datafields["objPose_y"] = new Variant(buf.objPose_y);
-        datafields["objPose_z"] = new Variant(buf.objPose_z);
-
-        datafields["objForce_x"] = new Variant(buf.objForce_x);
-        datafields["objForce_y"] = new Variant(buf.objForce_y);
-        datafields["objForce_z"] = new Variant(buf.objForce_z);
-        datafields["objTorque_x"] = new Variant(buf.objTorque_x);
-        datafields["objTorque_y"] = new Variant(buf.objTorque_y);
-        datafields["objTorque_z"] = new Variant(buf.objTorque_z);
-
-        datafields["objVel_x"] = new Variant(buf.objVel_x);
-        datafields["objVel_y"] = new Variant(buf.objVel_y);
-        datafields["objVel_z"] = new Variant(buf.objVel_z);
-        datafields["objVel_rx"] = new Variant(buf.objVel_rx);
-        datafields["objVel_ry"] = new Variant(buf.objVel_ry);
-        datafields["objVel_rz"] = new Variant(buf.objVel_rz);
-
-        datafields["deltaPose_x"] = new Variant(buf.deltaPose_x);
-        datafields["deltaPose_y"] = new Variant(buf.deltaPose_y);
-        datafields["deltaPose_z"] = new Variant(buf.deltaPose_z);
-        datafields["deltaPose_rx"] = new Variant(buf.deltaPose_rx);
-        datafields["deltaPose_ry"] = new Variant(buf.deltaPose_ry);
-        datafields["deltaPose_rz"] = new Variant(buf.deltaPose_rz);
-
-        datafields["modifiedPoseRight_x"] = new Variant(buf.modifiedPoseRight_x);
-        datafields["modifiedPoseRight_y"] = new Variant(buf.modifiedPoseRight_y);
-        datafields["modifiedPoseRight_z"] = new Variant(buf.modifiedPoseRight_z);
-        datafields["currentPoseLeft_x"] = new Variant(buf.currentPoseLeft_x);
-        datafields["currentPoseLeft_y"] = new Variant(buf.currentPoseLeft_y);
-        datafields["currentPoseLeft_z"] = new Variant(buf.currentPoseLeft_z);
-
-
-        datafields["modifiedPoseLeft_x"] = new Variant(buf.modifiedPoseLeft_x);
-        datafields["modifiedPoseLeft_y"] = new Variant(buf.modifiedPoseLeft_y);
-        datafields["modifiedPoseLeft_z"] = new Variant(buf.modifiedPoseLeft_z);
-
-        datafields["currentPoseRight_x"] = new Variant(buf.currentPoseRight_x);
-        datafields["currentPoseRight_y"] = new Variant(buf.currentPoseRight_y);
-        datafields["currentPoseRight_z"] = new Variant(buf.currentPoseRight_z);
-        datafields["dmpBoxPose_x"] = new Variant(buf.dmpBoxPose_x);
-        datafields["dmpBoxPose_y"] = new Variant(buf.dmpBoxPose_y);
-        datafields["dmpBoxPose_z"] = new Variant(buf.dmpBoxPose_z);
-        datafields["dmpTwist_x"] = new Variant(buf.dmpTwist_x);
-        datafields["dmpTwist_y"] = new Variant(buf.dmpTwist_y);
-        datafields["dmpTwist_z"] = new Variant(buf.dmpTwist_z);
-
-        datafields["modifiedTwist_lx"] = new Variant(buf.modifiedTwist_lx);
-        datafields["modifiedTwist_ly"] = new Variant(buf.modifiedTwist_ly);
-        datafields["modifiedTwist_lz"] = new Variant(buf.modifiedTwist_lz);
-        datafields["modifiedTwist_rx"] = new Variant(buf.modifiedTwist_rx);
-        datafields["modifiedTwist_ry"] = new Variant(buf.modifiedTwist_ry);
-        datafields["modifiedTwist_rz"] = new Variant(buf.modifiedTwist_rz);
-
-        datafields["rx"] = new Variant(buf.rx);
-        datafields["ry"] = new Variant(buf.ry);
-        datafields["rz"] = new Variant(buf.rz);
-
-
-        debugObs->setDebugChannel("BimanualForceController", datafields);
-    }
-
-    Eigen::Matrix4f NJointBimanualCartesianAdmittanceController::getBoxPose(const Ice::Current&) const
-    {
-        std::lock_guard guard{debugOutputDataReadMutex};
-        return debugOutputData.getUpToDateReadBuffer().currentBoxPose;
-    }
-
-    void NJointBimanualCartesianAdmittanceController::setBoxPose(const Eigen::Matrix4f& pose, const Ice::Current&)
-    {
-        ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
-        std::lock_guard guard{targBufWriteMutex};
-        targBuf.getWriteBuffer().pose = pose;
-        targBuf.commitWrite();
-    }
-
-    void NJointBimanualCartesianAdmittanceController::setBoxWidth(float w, const Ice::Current&)
-    {
-        ARMARX_CHECK_GREATER_EQUAL(w, 0);
-        std::lock_guard{cfgBufWriteMutex};
-        cfgBuf.getWriteBuffer().boxWidth = w;
-        cfgBuf.commitWrite();
-    }
-    void NJointBimanualCartesianAdmittanceController::setBoxVelocity(
-        const Eigen::Vector3f& velXYZ,
-        const Eigen::Vector3f& velRPY,
-        const Ice::Current&)
-    {
-        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
-        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
-        std::lock_guard guard{targBufWriteMutex};
-        targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
-        targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
-        targBuf.commitWrite();
-
-    }
-    void NJointBimanualCartesianAdmittanceController::setBoxPoseAndVelocity(
-        const Eigen::Matrix4f& pose,
-        const Eigen::Vector3f& velXYZ,
-        const Eigen::Vector3f& velRPY, const Ice::Current&)
-    {
-        ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
-        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
-        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
-        std::lock_guard guard{targBufWriteMutex};
-        targBuf.getWriteBuffer().pose = pose;
-        targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
-        targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
-        targBuf.commitWrite();
-    }
-    void NJointBimanualCartesianAdmittanceController::moveBoxPose(const Eigen::Matrix4f& pose, const Ice::Current&)
-    {
-        ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
-        std::lock_guard guard{targBufWriteMutex};
-        const Eigen::Matrix4f tmp = pose * targBuf.getWriteBuffer().pose;
-        targBuf.getWriteBuffer().pose = tmp;
-        targBuf.commitWrite();
-    }
-    void NJointBimanualCartesianAdmittanceController::moveBoxPosition(const Eigen::Vector3f& pos, const Ice::Current&)
-    {
-        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(pos));
-        std::lock_guard guard{targBufWriteMutex};
-        targBuf.getWriteBuffer().pose.topRightCorner<3, 1>() += pos;
-        targBuf.commitWrite();
-    }
-}
-
-//set config
-namespace armarx
-{
-    void NJointBimanualCartesianAdmittanceController::setConfig(const NJointBimanualCartesianAdmittanceControllerConfigPtr& ptr, const Ice::Current&)
-    {
-        ARMARX_CHECK_NOT_NULL(ptr);
-        ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesLeft.size(), rt.left.targets.size());
-        ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesRight.size(), rt.right.targets.size());
-        std::lock_guard{cfgBufWriteMutex};
-        updateConfig(*ptr);
-        cfgBuf.commitWrite();
-    }
-    void NJointBimanualCartesianAdmittanceController::setDesiredJointValuesLeft(const Ice::FloatSeq& vals, const Ice::Current&)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        updateDesiredJointValuesLeft(vals);
-        cfgBuf.commitWrite();
-    }
-    void NJointBimanualCartesianAdmittanceController::setDesiredJointValuesRight(const Ice::FloatSeq& vals, const Ice::Current&)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        updateDesiredJointValuesRight(vals);
-        cfgBuf.commitWrite();
-    }
-    void NJointBimanualCartesianAdmittanceController::setNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace, const Ice::Current&)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        updateNullspaceConfig(nullspace);
-        cfgBuf.commitWrite();
-    }
-    void NJointBimanualCartesianAdmittanceController::setAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance& admittanceObject, const Ice::Current&)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        updateAdmittanceConfig(admittanceObject);
-        cfgBuf.commitWrite();
-    }
-    void NJointBimanualCartesianAdmittanceController::setForceConfig(const detail::NJBmanCartAdmCtrl::Force& left, const detail::NJBmanCartAdmCtrl::Force& right, const Ice::Current&)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        updateForceConfig(left, right);
-        cfgBuf.commitWrite();
-    }
-    void NJointBimanualCartesianAdmittanceController::setImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& left, const detail::NJBmanCartAdmCtrl::Impedance& right, const Ice::Current&)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        updateImpedanceConfig(left, right);
-        cfgBuf.commitWrite();
-    }
-}
-
-//update config without updating the buffer
-namespace armarx
-{
-    void NJointBimanualCartesianAdmittanceController::updateConfig(const NJointBimanualCartesianAdmittanceControllerConfig& cfg)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        auto& buf = cfgBuf.getWriteBuffer();
-        updateNullspaceConfig(cfg.nullspace);
-        updateAdmittanceConfig(cfg.admittanceObject);
-        updateForceConfig(cfg.forceLeft, cfg.forceRight);
-        updateImpedanceConfig(cfg.impedanceLeft, cfg.impedanceRight);
-        buf.torqueLimit = cfg.torqueLimit;
-        buf.filterCoeff = cfg.filterCoeff;
-        buf.ftCalibrationTime = cfg.ftCalibrationTime;
-        buf.boxWidth = cfg.box.width;
-    }
-    void NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesLeft(const Ice::FloatSeq& vals)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        auto& buf = cfgBuf.getWriteBuffer();
-        ARMARX_CHECK_EQUAL(vals.size(), rt.left.targets.size());
-        buf.desiredJointValuesLeft = Eigen::Map<const Eigen::VectorXf>(
-                                         vals.data(), vals.size());
-    }
-    void NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesRight(const Ice::FloatSeq& vals)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        auto& buf = cfgBuf.getWriteBuffer();
-        ARMARX_CHECK_EQUAL(vals.size(), rt.right.targets.size());
-        buf.desiredJointValuesRight = Eigen::Map<const Eigen::VectorXf>(
-                                          vals.data(), vals.size());
-    }
-    void NJointBimanualCartesianAdmittanceController::updateNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        auto& cfg = cfgBuf.getWriteBuffer();
-        updateDesiredJointValuesLeft(nullspace.desiredJointValuesLeft);
-        updateDesiredJointValuesRight(nullspace.desiredJointValuesRight);
-        cfg.knull = nullspace.k;
-        cfg.dnull = nullspace.d;
-    }
-    void NJointBimanualCartesianAdmittanceController::updateAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance admittanceObject)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        auto& cfg = cfgBuf.getWriteBuffer();
-        cfg.KmAdmittance.block<3, 1>(0, 0) = admittanceObject.KmXYZ;
-        cfg.KmAdmittance.block<3, 1>(3, 0) = admittanceObject.KmRPY;
-
-        cfg.KpAdmittance.block<3, 1>(0, 0) = admittanceObject.KpXYZ;
-        cfg.KpAdmittance.block<3, 1>(3, 0) = admittanceObject.KpRPY;
-
-        cfg.KdAdmittance.block<3, 1>(0, 0) = admittanceObject.KdXYZ;
-        cfg.KdAdmittance.block<3, 1>(3, 0) = admittanceObject.KdRPY;
-    }
-    void NJointBimanualCartesianAdmittanceController::updateForceConfig(const detail::NJBmanCartAdmCtrl::Force& forceLeft, const detail::NJBmanCartAdmCtrl::Force& forceRight)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        auto& cfg = cfgBuf.getWriteBuffer();
-        //left
-        cfg.massLeft = forceLeft.mass;
-        cfg.CoMVecLeft = forceLeft.com;
-        cfg.forceOffsetLeft = forceLeft.offsetForce;
-        cfg.torqueOffsetLeft = forceLeft.offsetTorque;
-        cfg.targetWrench.block<3, 1>(0, 0) = forceLeft.wrenchXYZ;
-        cfg.targetWrench.block<3, 1>(3, 0) = forceLeft.wrenchRPY;
-        cfg.forceThreshold.block<3, 1>(0, 0) = forceLeft.forceThreshold;
-        //right
-        cfg.massRight = forceRight.mass;
-        cfg.CoMVecRight = forceRight.com;
-        cfg.forceOffsetRight = forceRight.offsetForce;
-        cfg.torqueOffsetRight = forceRight.offsetTorque;
-        cfg.targetWrench.block<3, 1>(6, 0) = forceRight.wrenchXYZ;
-        cfg.targetWrench.block<3, 1>(9, 0) = forceRight.wrenchRPY;
-        cfg.forceThreshold.block<3, 1>(3, 0) = forceRight.forceThreshold;
-    }
-    void NJointBimanualCartesianAdmittanceController::updateImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& impedanceLeft,
-            const detail::NJBmanCartAdmCtrl::Impedance& impedanceRight)
-    {
-        std::lock_guard{cfgBufWriteMutex};
-        auto& cfg = cfgBuf.getWriteBuffer();
-        cfg.KpImpedance.block<3, 1>(0, 0) = impedanceLeft.KpXYZ;
-        cfg.KpImpedance.block<3, 1>(3, 0) = impedanceLeft.KpRPY;
-        cfg.KpImpedance.block<3, 1>(6, 0) = impedanceRight.KpXYZ;
-        cfg.KpImpedance.block<3, 1>(9, 0) = impedanceRight.KpRPY;
-
-        cfg.KdImpedance.block<3, 1>(0, 0) = impedanceLeft.KdXYZ;
-        cfg.KdImpedance.block<3, 1>(3, 0) = impedanceLeft.KdRPY;
-        cfg.KdImpedance.block<3, 1>(6, 0) = impedanceRight.KdXYZ;
-        cfg.KdImpedance.block<3, 1>(9, 0) = impedanceRight.KdRPY;
-    }
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h
deleted file mode 100644
index 3a10a4bb32861310989f309391485f389d7b083f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h
+++ /dev/null
@@ -1,278 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
-    TYPEDEF_PTRS_HANDLE(NJointBimanualCartesianAdmittanceController);
-    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelControlData);
-
-
-    class NJointBimanualCartesianAdmittanceController :
-        public NJointController,
-        public NJointBimanualCartesianAdmittanceControllerInterface
-    {
-    public:
-        NJointBimanualCartesianAdmittanceController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointController interface
-        std::string getClassName(const Ice::Current&) const override;
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        //set config
-        void setConfig(const NJointBimanualCartesianAdmittanceControllerConfigPtr& ptr, const Ice::Current& = Ice::emptyCurrent) override;
-        void setDesiredJointValuesLeft(const Ice::FloatSeq& vals, const Ice::Current& = Ice::emptyCurrent) override;
-        void setDesiredJointValuesRight(const Ice::FloatSeq& vals, const Ice::Current& = Ice::emptyCurrent) override;
-        void setNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace, const Ice::Current& = Ice::emptyCurrent) override;
-        void setAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance& admittanceObject, const Ice::Current& = Ice::emptyCurrent) override;
-        void setForceConfig(const detail::NJBmanCartAdmCtrl::Force& left, const detail::NJBmanCartAdmCtrl::Force& right, const Ice::Current& = Ice::emptyCurrent) override;
-        void setImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& left, const detail::NJBmanCartAdmCtrl::Impedance& right, const Ice::Current& = Ice::emptyCurrent) override;
-        //control
-        Eigen::Matrix4f getBoxPose(const Ice::Current& = Ice::emptyCurrent) const override;
-        void setBoxPose(const Eigen::Matrix4f& pose, const Ice::Current& = Ice::emptyCurrent) override;
-        void setBoxWidth(float w, const Ice::Current& = Ice::emptyCurrent) override;
-        void setBoxVelocity(
-            const Eigen::Vector3f& velXYZ,
-            const Eigen::Vector3f& velRPY,
-            const Ice::Current& = Ice::emptyCurrent) override;
-        void setBoxPoseAndVelocity(const Eigen::Matrix4f& pose,
-                                   const Eigen::Vector3f& velXYZ,
-                                   const Eigen::Vector3f& velRPY,
-                                   const Ice::Current& = Ice::emptyCurrent) override;
-        void moveBoxPose(const Eigen::Matrix4f& pose, const Ice::Current& = Ice::emptyCurrent) override;
-        void moveBoxPosition(const Eigen::Vector3f& pos, const Ice::Current& = Ice::emptyCurrent) override;
-    protected:
-        void updateConfig(const NJointBimanualCartesianAdmittanceControllerConfig& cfg);
-        void updateDesiredJointValuesLeft(const Ice::FloatSeq& cfg);
-        void updateDesiredJointValuesRight(const Ice::FloatSeq& cfg);
-        void updateNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace);
-        void updateAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance admittanceObject);
-        void updateForceConfig(const detail::NJBmanCartAdmCtrl::Force& left, const detail::NJBmanCartAdmCtrl::Force& right);
-        void updateImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& left, const detail::NJBmanCartAdmCtrl::Impedance& right);
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        //        void onInitNJointController();
-        //        void onDisconnectNJointController();
-        //        void controllerRun(); //runs dmp controller
-
-    private:
-        struct Target
-        {
-            Eigen::Matrix4f pose;
-            Eigen::Vector6f vel;
-        };
-        mutable std::recursive_mutex      targBufWriteMutex;
-        WriteBufferedTripleBuffer<Target> targBuf;
-
-
-        struct PreprocessedCfg
-        {
-            float boxWidth;
-
-            Eigen::Vector6f KmAdmittance;
-            Eigen::Vector6f KpAdmittance;
-            Eigen::Vector6f KdAdmittance;
-
-            float ftCalibrationTime;
-
-            Eigen::Vector12f KpImpedance;
-            Eigen::Vector12f KdImpedance;
-
-            float massLeft;
-            Eigen::Vector3f CoMVecLeft;
-            Eigen::Vector3f forceOffsetLeft;
-            Eigen::Vector3f torqueOffsetLeft;
-
-            float massRight;
-            Eigen::Vector3f CoMVecRight;
-            Eigen::Vector3f forceOffsetRight;
-            Eigen::Vector3f torqueOffsetRight;
-
-            Eigen::VectorXf desiredJointValuesLeft;
-            Eigen::VectorXf desiredJointValuesRight;
-
-            float knull;
-            float dnull;
-
-            float torqueLimit;
-
-            Eigen::Vector12f targetWrench;
-
-            float filterCoeff;
-
-            Eigen::Vector6f forceThreshold;
-        };
-        mutable std::recursive_mutex               cfgBufWriteMutex;
-        WriteBufferedTripleBuffer<PreprocessedCfg> cfgBuf;
-
-        struct RTData
-        {
-            struct Arm
-            {
-                std::vector<ControlTarget1DoFActuatorTorque*> targets;
-                std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
-                std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-                const SensorValueForceTorque* forceTorque;
-                VirtualRobot::DifferentialIKPtr IK;
-
-                std::vector<std::string> jointNames;
-                VirtualRobot::RobotNodeSetPtr rns;
-                VirtualRobot::RobotNodePtr tcp;
-                VirtualRobot::RobotNodePtr frameFTSensor;
-
-                Eigen::Matrix4f sensorFrame2TcpFrame{Eigen::Matrix4f::Identity()};
-            };
-            Arm left;
-            Arm right;
-
-            double ftcalibrationTimer = 0;
-            //            Eigen::Vector12f ftOffset = Eigen::Vector12f::Zero();
-            bool firstLoop = true;
-
-            Eigen::Vector6f virtualAcc = Eigen::Vector6f::Zero();
-            Eigen::Vector6f virtualVel = Eigen::Vector6f::Zero();
-            Eigen::Matrix4f virtualPose = Eigen::Matrix4f::Identity();
-
-            Eigen::Vector12f filteredOldValue = Eigen::Vector12f::Zero();
-        };
-        RTData rt;
-
-
-        struct DebugBufferData
-        {
-            Eigen::Matrix4f currentBoxPose;
-
-            StringFloatDictionary desired_torques;
-
-            float virtualPose_x;
-            float virtualPose_y;
-            float virtualPose_z;
-
-            float objPose_x;
-            float objPose_y;
-            float objPose_z;
-
-            float objForce_x;
-            float objForce_y;
-            float objForce_z;
-            float objTorque_x;
-            float objTorque_y;
-            float objTorque_z;
-
-            float deltaPose_x;
-            float deltaPose_y;
-            float deltaPose_z;
-            float deltaPose_rx;
-            float deltaPose_ry;
-            float deltaPose_rz;
-
-            float objVel_x;
-            float objVel_y;
-            float objVel_z;
-            float objVel_rx;
-            float objVel_ry;
-            float objVel_rz;
-
-            float modifiedPoseRight_x;
-            float modifiedPoseRight_y;
-            float modifiedPoseRight_z;
-            float currentPoseLeft_x;
-            float currentPoseLeft_y;
-            float currentPoseLeft_z;
-
-            float modifiedPoseLeft_x;
-            float modifiedPoseLeft_y;
-            float modifiedPoseLeft_z;
-            float currentPoseRight_x;
-            float currentPoseRight_y;
-            float currentPoseRight_z;
-
-            float dmpBoxPose_x;
-            float dmpBoxPose_y;
-            float dmpBoxPose_z;
-
-            float dmpTwist_x;
-            float dmpTwist_y;
-            float dmpTwist_z;
-
-            float modifiedTwist_lx;
-            float modifiedTwist_ly;
-            float modifiedTwist_lz;
-            float modifiedTwist_rx;
-            float modifiedTwist_ry;
-            float modifiedTwist_rz;
-
-            float rx;
-            float ry;
-            float rz;
-
-            //            Eigen::VectorXf wrenchDMP;
-            //            Eigen::VectorXf computedBoxWrench;
-
-            Eigen::VectorXf forceImpedance;
-            Eigen::VectorXf forcePID;
-            Eigen::VectorXf forcePIDControlValue;
-            Eigen::VectorXf poseError;
-            Eigen::VectorXf wrenchesConstrained;
-            Eigen::VectorXf wrenchesMeasuredInRoot;
-        };
-
-        mutable std::recursive_mutex  debugOutputDataReadMutex;
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-        //        struct rt2ControlData
-        //        {
-        //            double currentTime;
-        //            double deltaT;
-        //            Eigen::Matrix4f currentPose;
-        //            Eigen::VectorXf currentTwist;
-        //        };
-        //        TripleBuffer<rt2ControlData> rt2ControlBuffer;
-
-        //        struct ControlInterfaceData
-        //        {
-        //            Eigen::Matrix4f currentLeftPose;
-        //            Eigen::Matrix4f currentRightPose;
-        //        };
-
-        //        TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
-
-        //        float torqueLimit;
-
-
-
-        //        TaskSpaceDMPControllerPtr objectDMP;
-
-
-        //        Eigen::Matrix4f leftInitialPose;
-        //        Eigen::Matrix4f rightInitialPose;
-        //        Eigen::Matrix4f boxInitialPose;
-
-
-        //        std::vector<PIDControllerPtr> forcePIDControllers;
-
-        // filter parameters
-        //        bool finished;
-        //        bool dmpStarted;
-    protected:
-        void rtPreActivateController();
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
deleted file mode 100644
index 4130b956b90dbae919c281d7ec9eacea13a3880c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
+++ /dev/null
@@ -1,869 +0,0 @@
-#include "NJointBimanualForceController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointBimanualForceController> registrationControllerNJointBimanualForceController("NJointBimanualForceController");
-
-    NJointBimanualForceController::NJointBimanualForceController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "Preparing ... bimanual ";
-        useSynchronizedRtRobot();
-        cfg = NJointBimanualForceControllerConfigPtr::dynamicCast(config);
-        //        ARMARX_CHECK_EXPRESSION(prov);
-        //        RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
-        //        ARMARX_CHECK_EXPRESSION(robotUnit);
-        leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-
-        for (size_t i = 0; i < leftRNS->getSize(); ++i)
-        {
-            std::string jointName = leftRNS->getNode(i)->getName();
-            leftJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-
-            leftVelocitySensors.push_back(velocitySensor);
-            leftPositionSensors.push_back(positionSensor);
-
-        };
-
-        rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
-        for (size_t i = 0; i < rightRNS->getSize(); ++i)
-        {
-            std::string jointName = rightRNS->getNode(i)->getName();
-            rightJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-            rightVelocitySensors.push_back(velocitySensor);
-            rightPositionSensors.push_back(positionSensor);
-
-        };
-
-
-        //        const SensorValueBase* svlf = prov->getSensorValue("FT L");
-        const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
-        leftForceTorque = svlf->asA<SensorValueForceTorque>();
-        //        const SensorValueBase* svrf = prov->getSensorValue("FT R");
-        const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
-        rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
-        leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
-        taskSpaceDMPConfig.DMPAmplitude = 1.0;
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-
-
-        objectDMP.reset(new TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
-        ARMARX_IMPORTANT << "dmp finieshed";
-
-        tcpLeft = leftRNS->getTCP();
-        tcpRight = rightRNS->getTCP();
-
-        KpImpedance.resize(cfg->KpImpedance.size());
-        ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
-
-        for (int i = 0; i < KpImpedance.size(); ++i)
-        {
-            KpImpedance(i) = cfg->KpImpedance.at(i);
-        }
-
-        KdImpedance.resize(cfg->KdImpedance.size());
-        ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 6);
-
-        for (int i = 0; i < KdImpedance.size(); ++i)
-        {
-            KdImpedance(i) = cfg->KdImpedance.at(i);
-        }
-
-        KpAdmittance.resize(cfg->KpAdmittance.size());
-        ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
-
-        for (int i = 0; i < KpAdmittance.size(); ++i)
-        {
-            KpAdmittance(i) = cfg->KpAdmittance.at(i);
-        }
-
-        KdAdmittance.resize(cfg->KdAdmittance.size());
-        ARMARX_CHECK_EQUAL(cfg->KdAdmittance.size(), 6);
-
-        for (int i = 0; i < KdAdmittance.size(); ++i)
-        {
-            KdAdmittance(i) = cfg->KdAdmittance.at(i);
-        }
-
-        KmAdmittance.resize(cfg->KmAdmittance.size());
-        ARMARX_CHECK_EQUAL(cfg->KmAdmittance.size(), 6);
-
-        for (int i = 0; i < KmAdmittance.size(); ++i)
-        {
-            KmAdmittance(i) = cfg->KmAdmittance.at(i);
-        }
-
-        leftDesiredJointValues.resize(leftTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
-        }
-
-        rightDesiredJointValues.resize(rightTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
-        }
-
-        KmPID.resize(cfg->KmPID.size());
-        ARMARX_CHECK_EQUAL(cfg->KmPID.size(), 6);
-
-        for (int i = 0; i < KmPID.size(); ++i)
-        {
-            KmPID(i) = cfg->KmPID.at(i);
-        }
-
-
-
-        modifiedAcc.setZero(12);
-        modifiedTwist.setZero(12);
-        ARMARX_INFO << "got controller params";
-
-
-        boxInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->boxInitialPose[4], cfg->boxInitialPose[5], cfg->boxInitialPose[6], cfg->boxInitialPose[3]);
-        for (int i = 0; i < 3; ++i)
-        {
-            boxInitialPose(i, 3) = cfg->boxInitialPose[i];
-        }
-
-        NJointBimanualForceControllerSensorData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentPose = boxInitialPose;
-        initSensorData.currentTwist.setZero();
-        controllerSensorData.reinitAllBuffers(initSensorData);
-
-
-        NJointBimanualForceControllerInterfaceData initInterfaceData;
-        initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
-        initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
-        interfaceData.reinitAllBuffers(initInterfaceData);
-
-        leftInitialPose = boxInitialPose;
-        leftInitialPose(0, 3) -= cfg->boxWidth;
-        rightInitialPose = boxInitialPose;
-        rightInitialPose(0, 3) += cfg->boxWidth;
-
-        forcePIDControllers.resize(12);
-        for (size_t i = 0; i < 6; i++)
-        {
-            forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
-            forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
-            forcePIDControllers.at(i)->reset();
-            forcePIDControllers.at(i + 6)->reset();
-        }
-
-        // filter
-        filterCoeff = cfg->filterCoeff;
-        ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
-        filteredOldValue.setZero(12);
-
-        // static compensation
-        massLeft = cfg->massLeft;
-        CoMVecLeft << cfg->CoMVecLeft[0],  cfg->CoMVecLeft[1],  cfg->CoMVecLeft[2];
-        forceOffsetLeft << cfg->forceOffsetLeft[0],  cfg->forceOffsetLeft[1],  cfg->forceOffsetLeft[2];
-        torqueOffsetLeft << cfg->torqueOffsetLeft[0],  cfg->torqueOffsetLeft[1],  cfg->torqueOffsetLeft[2];
-
-        massRight = cfg->massRight;
-        CoMVecRight << cfg->CoMVecRight[0],  cfg->CoMVecRight[1],  cfg->CoMVecRight[2];
-        forceOffsetRight << cfg->forceOffsetRight[0],  cfg->forceOffsetRight[1],  cfg->forceOffsetRight[2];
-        torqueOffsetRight << cfg->torqueOffsetRight[0],  cfg->torqueOffsetRight[1],  cfg->torqueOffsetRight[2];
-
-        sensorFrame2TcpFrameLeft.setZero();
-        sensorFrame2TcpFrameRight.setZero();
-
-        NJointBimanualForceControlData initData;
-        initData.boxPose = boxInitialPose;
-        initData.boxTwist.setZero(6);
-        reinitTripleBuffer(initData);
-
-        firstLoop = true;
-        ARMARX_INFO << "left initial pose: \n" << leftInitialPose  << "\n right initial pose: \n" << rightInitialPose;
-
-        ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
-        ARMARX_IMPORTANT << "finished construction!";
-
-        dmpStarted = false;
-
-        targetWrench.setZero(cfg->targetWrench.size());
-        for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
-        {
-            targetWrench(i) = cfg->targetWrench[i];
-        }
-
-
-
-    }
-
-    std::string NJointBimanualForceController::getClassName(const Ice::Current&) const
-    {
-        return "NJointBimanualForceController";
-    }
-
-    //    void NJointBimanualForceController::rtPreActivateController()
-    //    {
-    //        //        modifiedLeftPose = tcpLeft->getPoseInRootFrame();
-    //        //        modifiedRightPose = tcpRight->getPoseInRootFrame();
-    //        //        Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
-    //        //        Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
-    //        //        sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = modifiedLeftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
-    //        //        sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = modifiedRightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
-    //        //        CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
-    //        //        CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
-    //    }
-
-
-    void NJointBimanualForceController::controllerRun()
-    {
-        if (!controllerSensorData.updateReadBuffer() || !dmpStarted)
-        {
-            return;
-        }
-
-        double deltaT = controllerSensorData.getReadBuffer().deltaT;
-        Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
-        Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
-
-        if (objectDMP->canVal < 1e-8)
-        {
-            finished = true;
-        }
-
-        objectDMP->flow(deltaT, currentPose, currentTwist);
-
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
-        getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
-        writeControlStruct();
-    }
-
-
-
-
-    void NJointBimanualForceController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        if (firstLoop)
-        {
-            modifiedLeftPose = tcpLeft->getPoseInRootFrame();
-            modifiedRightPose = tcpRight->getPoseInRootFrame();
-            modifiedLeftPose.block<3, 1>(0, 3) = modifiedLeftPose.block<3, 1>(0, 3) * 0.001;
-            modifiedRightPose.block<3, 1>(0, 3) = modifiedRightPose.block<3, 1>(0, 3) * 0.001;
-
-            Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
-            Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
-            leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
-            rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
-
-            sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = modifiedLeftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
-            sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = modifiedRightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
-            CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
-            CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
-            firstLoop = false;
-            ARMARX_INFO << "modified left pose:\n " << modifiedLeftPose;
-            ARMARX_INFO << "modified right pose:\n " << modifiedRightPose;
-        }
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-
-        // grasp matrix
-        Eigen::Vector3f rToBoxCoM;
-        rToBoxCoM << cfg->boxWidth, 0, 0;
-        Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
-
-        interfaceData.getWriteBuffer().currentLeftPose = currentLeftPose;
-        interfaceData.getWriteBuffer().currentRightPose = currentRightPose;
-        interfaceData.commitWrite();
-
-        Eigen::VectorXf currentTargetWrench = targetWrench;
-        if (fabs(currentLeftPose(0, 3) - currentRightPose(0, 3)) < 0.8 * 2 *  cfg->boxWidth)
-        {
-            currentTargetWrench.setZero();
-        }
-        currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
-        currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
-        Eigen::MatrixXf graspMatrix;
-        graspMatrix.setZero(6, 12);
-        graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
-        graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
-        Eigen::Vector3f r = - currentLeftPose.block<3, 3>(0, 0) * rToBoxCoM;
-        graspMatrix(4, 2) = -r(0);
-        graspMatrix(3, 2) = r(1);
-        graspMatrix(3, 1) = -r(2);
-        graspMatrix(4, 0) = r(2);
-        graspMatrix(5, 0) = -r(1);
-        graspMatrix(5, 1) = r(0);
-        r = currentRightPose.block<3, 3>(0, 0) * rToBoxCoM;
-        graspMatrix(4, 8) = -r(0);
-        graspMatrix(3, 8) = r(1);
-        graspMatrix(3, 7) = -r(2);
-        graspMatrix(4, 6) = r(2);
-        graspMatrix(5, 6) = -r(1);
-        graspMatrix(5, 7) = r(0);
-        // projection of grasp matrix
-        Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
-        Eigen::MatrixXf G_range = pinvG * graspMatrix;
-        Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
-
-        // box pose
-        Eigen::Matrix4f boxCurrentPose = currentLeftPose;
-        boxCurrentPose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
-        Eigen::VectorXf boxCurrentTwist;
-        boxCurrentTwist.setZero(6);
-
-        // cartesian vel controller
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-
-        Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf leftqpos;
-        Eigen::VectorXf leftqvel;
-        leftqpos.resize(leftPositionSensors.size());
-        leftqvel.resize(leftVelocitySensors.size());
-        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
-        {
-            leftqpos(i) = leftPositionSensors[i]->position;
-            leftqvel(i) = leftVelocitySensors[i]->velocity;
-        }
-
-        Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        // jacobiL used in L304
-        jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
-        jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
-        Eigen::VectorXf rightqpos;
-        Eigen::VectorXf rightqvel;
-        rightqpos.resize(rightPositionSensors.size());
-        rightqvel.resize(rightVelocitySensors.size());
-
-        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
-        {
-            rightqpos(i) = rightPositionSensors[i]->position;
-            rightqvel(i) = rightVelocitySensors[i]->velocity;
-        }
-
-        // what is the unit of jacobiL, 0.001?
-        Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
-        Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-        Eigen::VectorXf currentTwist(12);
-        currentTwist << currentLeftTwist, currentRightTwist;
-        Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), 0);
-        boxCurrentTwist = pinvGraspMatrixT * currentTwist;
-
-
-
-
-        controllerSensorData.getWriteBuffer().currentPose = boxCurrentPose;
-        controllerSensorData.getWriteBuffer().currentTwist = boxCurrentTwist;
-        controllerSensorData.getWriteBuffer().deltaT = deltaT;
-        controllerSensorData.getWriteBuffer().currentTime += deltaT;
-        controllerSensorData.commitWrite();
-
-
-
-
-        Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
-        Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
-
-        Eigen::VectorXf leftJointControlWrench;
-        Eigen::VectorXf rightJointControlWrench;
-
-
-
-        //Todo: calculate desired wrench from required box pose
-        //        Eigen::VectorXf boxPoseError(6);
-        //        Eigen::Matrix3f diffMat = boxPose.block<3, 3>(0, 0) * boxCurrentPose.block<3, 3>(0, 0).transpose();
-        //        boxPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-        //        boxPoseError.head(3) = boxPose.block<3, 1>(0, 3) - boxCurrentPose.block<3, 1>(0, 3);
-        //        Eigen::VectorXf computedBoxWrench(6);
-        //        computedBoxWrench = KpAdmittance.cwiseProduct(boxPoseError);// + KdAdmittance.cwiseProduct(boxTwist - boxCurrentTwist);
-        //        Eigen::VectorXf wrenchDMP = graspMatrix.transpose() * computedBoxWrench;
-        //        wrenchDMP.setZero();
-        Eigen::VectorXf twistDMP = graspMatrix.transpose() * boxTwist;
-        Eigen::VectorXf deltaInitialPose = deltaT * twistDMP;
-        leftInitialPose.block<3, 1>(0, 3)  = leftInitialPose.block<3, 1>(0, 3) + deltaInitialPose.block<3, 1>(0, 0);
-        rightInitialPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3) + deltaInitialPose.block<3, 1>(6, 0);
-        leftInitialPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaInitialPose(3), deltaInitialPose(4), deltaInitialPose(5)) * leftInitialPose.block<3, 3>(0, 0);
-        rightInitialPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaInitialPose(9), deltaInitialPose(10), deltaInitialPose(11)) * rightInitialPose.block<3, 3>(0, 0);
-
-
-
-        // static compensation
-        Eigen::Vector3f gravity;
-        gravity << 0.0, 0.0, -9.8;
-        Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
-        Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
-        Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
-
-        Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
-        Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
-        Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
-
-        // mapping of measured wrenches
-        Eigen::VectorXf wrenchesMeasured(12);
-        wrenchesMeasured << leftForceTorque->force - forceOffsetLeft, leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight, rightForceTorque->torque - torqueOffsetRight;
-        for (size_t i = 0; i < 12; i++)
-        {
-            wrenchesMeasured(i) = (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
-        }
-        filteredOldValue = wrenchesMeasured;
-        wrenchesMeasured.block<3, 1>(0, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
-        wrenchesMeasured.block<3, 1>(3, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
-        wrenchesMeasured.block<3, 1>(6, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
-        wrenchesMeasured.block<3, 1>(9, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
-        if (wrenchesMeasured.norm() < cfg->forceThreshold)
-        {
-            wrenchesMeasured.setZero();
-        }
-
-        // PID force controller
-        //        Eigen::VectorXf wrenchesConstrainedInLocal(12);
-        //        wrenchesConstrainedInLocal.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(0, 0);
-        //        wrenchesConstrainedInLocal.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(3, 0);
-        //        wrenchesConstrainedInLocal.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(6, 0);
-        //        wrenchesConstrainedInLocal.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(9, 0);
-        Eigen::VectorXf forcePID(12);
-        //        Eigen::VectorXf forcePIDControlValue(12);
-        //        for (size_t i = 0; i < 12; i++)
-        //        {
-        //            forcePIDControllers[i]->update(deltaT, wrenchesConstrainedInLocal(i), cfg->targetWrench[i]);
-        //            forcePIDControllers[i]->update(deltaT, wrenchesMeasured(i), cfg->targetWrench[i]);
-        //            forcePIDControlValue(i) = forcePIDControllers[i]->getControlValue();
-        //            forcePID(i) = - forcePIDControllers[i]->getControlValue();
-
-        //        }
-        for (size_t i = 0; i < 6; i++)
-        {
-            forcePID(i) = cfg->forceP[i] * (currentTargetWrench(i) - wrenchesMeasured(i));
-            forcePID(i + 6) = cfg->forceP[i] * (currentTargetWrench(i + 6) - wrenchesMeasured(i + 6));
-        }
-        Eigen::VectorXf forcePIDInRoot(12);
-        forcePIDInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(0, 0);
-        forcePIDInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(3, 0);
-        forcePIDInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(6, 0);
-        forcePIDInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(9, 0);
-
-        //        forcePIDInRoot = PG * forcePIDInRoot;
-        Eigen::VectorXf forcePIDInRootForDebug = forcePIDInRoot;
-
-        Eigen::VectorXf wrenchesMeasuredInRoot(12);
-        wrenchesMeasuredInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
-        wrenchesMeasuredInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
-        wrenchesMeasuredInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
-        wrenchesMeasuredInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
-        Eigen::VectorXf wrenchesConstrained = PG * wrenchesMeasuredInRoot;
-        //        wrenchesConstrained.setZero();
-
-
-
-
-        // admittance
-        Eigen::VectorXf poseError(12);
-        poseError.block<3, 1>(0, 0) = leftInitialPose.block<3, 1>(0, 3) - modifiedLeftPose.block<3, 1>(0, 3);
-        Eigen::Matrix3f diffMat = leftInitialPose.block<3, 3>(0, 0) * modifiedLeftPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-        poseError.block<3, 1>(6, 0) = rightInitialPose.block<3, 1>(0, 3) - modifiedRightPose.block<3, 1>(0, 3);
-        diffMat = rightInitialPose.block<3, 3>(0, 0) * modifiedRightPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        Eigen::VectorXf acc;
-        Eigen::VectorXf twist;
-        twist.setZero(12);
-        acc.setZero(12);
-        for (size_t i = 0; i < 6; i++)
-        {
-            //            acc(i) = KpAdmittance(i) * poseError(i) - KdAdmittance(i) * modifiedTwist(i) + wrenchDMP(i) - KmAdmittance(i) * wrenchesConstrained(i);
-            //            acc(i + 6) = KpAdmittance(i) * poseError(i + 6) - KdAdmittance(i) * modifiedTwist(i + 6) + wrenchDMP(i + 6) - KmAdmittance(i) * wrenchesConstrained(i + 6);
-            acc(i) = KpAdmittance(i) * poseError(i) - KdAdmittance(i) * modifiedTwist(i) - KmAdmittance(i) * wrenchesConstrained(i) - KmPID(i) * forcePIDInRoot(i);
-            acc(i + 6) = KpAdmittance(i) * poseError(i + 6) - KdAdmittance(i) * modifiedTwist(i + 6) - KmAdmittance(i) * wrenchesConstrained(i + 6) - KmPID(i) * forcePIDInRoot(i + 6);
-        }
-        twist = modifiedTwist + 0.5 * deltaT * (acc + modifiedAcc);
-        Eigen::VectorXf deltaPose = 0.5 * deltaT * (twist + modifiedTwist);
-        modifiedAcc = acc;
-        modifiedTwist = twist;
-
-        modifiedLeftPose.block<3, 1>(0, 3)  = modifiedLeftPose.block<3, 1>(0, 3) + deltaPose.block<3, 1>(0, 0);
-        modifiedRightPose.block<3, 1>(0, 3) = modifiedRightPose.block<3, 1>(0, 3) + deltaPose.block<3, 1>(6, 0);
-        modifiedLeftPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5)) * modifiedLeftPose.block<3, 3>(0, 0);
-        modifiedRightPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaPose(9), deltaPose(10), deltaPose(11)) * modifiedRightPose.block<3, 3>(0, 0);
-
-        //        for (size_t i = 0; i < 6; i++)
-        //        {
-        //            poseError(i) = (wrenchDMP(i) + wrenchesConstrained(i)) / KpAdmittance(i);
-        //            poseError(i + 6) = (wrenchDMP(i + 6) + wrenchesConstrained(i + 6)) / KpAdmittance(i);
-        //        }
-        //        modifiedLeftPose.block<3, 1>(0, 3)  = leftInitialPose.block<3, 1>(0, 3) + poseError.block<3, 1>(0, 0);
-        //        modifiedRightPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3) + poseError.block<3, 1>(6, 0);
-        //        modifiedLeftPose.block<3, 3>(0, 0) =  VirtualRobot::MathTools::rpy2eigen3f(poseError(3), poseError(4), poseError(5)) * leftInitialPose.block<3, 3>(0, 0);
-        //        modifiedRightPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(poseError(9), poseError(10), poseError(11)) * rightInitialPose.block<3, 3>(0, 0) * ;
-
-        // impedance control
-        diffMat = modifiedLeftPose.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(0, 0) = modifiedLeftPose.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
-        poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        diffMat = modifiedRightPose.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(6, 0) = modifiedRightPose.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
-        poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        Eigen::VectorXf forceImpedance(12);
-        for (size_t i = 0; i < 6; i++)
-        {
-            forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
-            forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
-        }
-
-
-
-        // nullspace
-        Eigen::VectorXf leftNullspaceTorque = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
-        Eigen::VectorXf rightNullspaceTorque = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
-
-        float lambda = 1;
-
-        //        forcePIDInRoot.setZero();
-        forcePIDInRoot.setZero();
-        leftJointControlWrench = forceImpedance.head(6) + forcePIDInRoot.head(6);
-        rightJointControlWrench = forceImpedance.tail(6) + forcePIDInRoot.tail(6);
-        Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
-        Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
-        Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
-        Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
-
-
-        // torque limit
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            float desiredTorque =   leftJointDesiredTorques(i);
-
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-
-            desiredTorque = (desiredTorque >  cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
-
-            debugOutputData.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
-
-            leftTargets.at(i)->torque = desiredTorque;
-        }
-
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            float desiredTorque = rightJointDesiredTorques(i);
-
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-
-            desiredTorque = (desiredTorque >   cfg->torqueLimit) ?  cfg->torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < - cfg->torqueLimit) ? - cfg->torqueLimit : desiredTorque;
-
-            debugOutputData.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
-
-            rightTargets.at(i)->torque = desiredTorque;
-        }
-        //        debugOutputData.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
-
-
-
-        debugOutputData.getWriteBuffer().forceImpedance = forceImpedance;
-        debugOutputData.getWriteBuffer().poseError = poseError;
-        debugOutputData.getWriteBuffer().wrenchesConstrained = wrenchesConstrained;
-        debugOutputData.getWriteBuffer().wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
-        //        debugOutputData.getWriteBuffer().wrenchDMP = wrenchDMP;
-        //        debugOutputData.getWriteBuffer().computedBoxWrench = computedBoxWrench;
-
-        debugOutputData.getWriteBuffer().modifiedPoseRight_x = modifiedRightPose(0, 3);
-        debugOutputData.getWriteBuffer().modifiedPoseRight_y = modifiedRightPose(1, 3);
-        debugOutputData.getWriteBuffer().modifiedPoseRight_z = modifiedRightPose(2, 3);
-
-        debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
-
-
-
-        debugOutputData.getWriteBuffer().modifiedPoseLeft_x = modifiedLeftPose(0, 3);
-        debugOutputData.getWriteBuffer().modifiedPoseLeft_y = modifiedLeftPose(1, 3);
-        debugOutputData.getWriteBuffer().modifiedPoseLeft_z = modifiedLeftPose(2, 3);
-
-        debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
-
-
-        debugOutputData.getWriteBuffer().dmpBoxPose_x = boxPose(0, 3);
-        debugOutputData.getWriteBuffer().dmpBoxPose_y = boxPose(1, 3);
-        debugOutputData.getWriteBuffer().dmpBoxPose_z = boxPose(2, 3);
-
-        debugOutputData.getWriteBuffer().dmpTwist_x = boxTwist(0);
-        debugOutputData.getWriteBuffer().dmpTwist_y = boxTwist(1);
-        debugOutputData.getWriteBuffer().dmpTwist_z = boxTwist(2);
-        debugOutputData.getWriteBuffer().rx = r(0);
-        debugOutputData.getWriteBuffer().ry = r(1);
-        debugOutputData.getWriteBuffer().rz = r(2);
-
-        debugOutputData.getWriteBuffer().modifiedTwist_lx = twistDMP(0);
-        debugOutputData.getWriteBuffer().modifiedTwist_ly = twistDMP(1);
-        debugOutputData.getWriteBuffer().modifiedTwist_lz = twistDMP(2);
-        debugOutputData.getWriteBuffer().modifiedTwist_rx = twistDMP(6);
-        debugOutputData.getWriteBuffer().modifiedTwist_ry = twistDMP(7);
-        debugOutputData.getWriteBuffer().modifiedTwist_rz = twistDMP(8);
-
-        debugOutputData.getWriteBuffer().forcePID = forcePIDInRootForDebug;
-
-        debugOutputData.commitWrite();
-
-    }
-
-    void NJointBimanualForceController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        objectDMP->learnDMPFromFiles(fileNames);
-    }
-
-
-    void NJointBimanualForceController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard(controllerMutex);
-        objectDMP->setGoalPoseVec(goals);
-
-    }
-
-
-    void NJointBimanualForceController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
-    {
-        while (!interfaceData.updateReadBuffer())
-        {
-            usleep(1000);
-        }
-
-        Eigen::Matrix4f leftPose = interfaceData.getUpToDateReadBuffer().currentLeftPose;
-        Eigen::Matrix4f rightPose = interfaceData.getUpToDateReadBuffer().currentRightPose;
-
-        VirtualRobot::MathTools::Quaternion boxOri = VirtualRobot::MathTools::eigen4f2quat(leftPose);
-        Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
-
-        std::vector<double> boxInitialPose;
-        for (size_t i = 0; i < 3; ++i)
-        {
-            boxInitialPose.push_back(boxPosi(i) * 0.001); //Important: mm -> m
-        }
-        boxInitialPose.push_back(boxOri.w);
-        boxInitialPose.push_back(boxOri.x);
-        boxInitialPose.push_back(boxOri.y);
-        boxInitialPose.push_back(boxOri.z);
-
-        virtualtimer = cfg->timeDuration;
-        objectDMP->prepareExecution(boxInitialPose, goals);
-
-        finished = false;
-        dmpStarted = true;
-    }
-
-    void NJointBimanualForceController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-        //        LockGuardType guard(controllerMutex);
-        ARMARX_INFO << "setting via points ";
-        objectDMP->setViaPose(u, viapoint);
-
-    }
-
-    void NJointBimanualForceController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        Eigen::VectorXf forceImpedance = debugOutputData.getUpToDateReadBuffer().forceImpedance;
-        for (int i = 0; i < forceImpedance.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "forceImpedance_" + ss.str();
-            datafields[data_name] = new Variant(forceImpedance(i));
-        }
-
-        Eigen::VectorXf forcePID = debugOutputData.getUpToDateReadBuffer().forcePID;
-        for (int i = 0; i < forcePID.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "forcePID_" + ss.str();
-            datafields[data_name] = new Variant(forcePID(i));
-        }
-
-
-        Eigen::VectorXf poseError = debugOutputData.getUpToDateReadBuffer().poseError;
-        for (int i = 0; i < poseError.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "poseError_" + ss.str();
-            datafields[data_name] = new Variant(poseError(i));
-        }
-
-        Eigen::VectorXf wrenchesConstrained = debugOutputData.getUpToDateReadBuffer().wrenchesConstrained;
-        for (int i = 0; i < wrenchesConstrained.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "wrenchesConstrained_" + ss.str();
-            datafields[data_name] = new Variant(wrenchesConstrained(i));
-        }
-
-        Eigen::VectorXf wrenchesMeasuredInRoot = debugOutputData.getUpToDateReadBuffer().wrenchesMeasuredInRoot;
-        for (int i = 0; i < wrenchesMeasuredInRoot.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "wrenchesMeasuredInRoot_" + ss.str();
-            datafields[data_name] = new Variant(wrenchesMeasuredInRoot(i));
-        }
-
-
-        //        Eigen::VectorXf wrenchDMP = debugOutputData.getUpToDateReadBuffer().wrenchDMP;
-        //        for (size_t i = 0; i < wrenchDMP.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "wrenchDMP_" + ss.str();
-        //            datafields[data_name] = new Variant(wrenchDMP(i));
-        //        }
-
-        //        Eigen::VectorXf computedBoxWrench = debugOutputData.getUpToDateReadBuffer().computedBoxWrench;
-        //        for (size_t i = 0; i < computedBoxWrench.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "computedBoxWrench_" + ss.str();
-        //            datafields[data_name] = new Variant(computedBoxWrench(i));
-        //        }
-
-
-        datafields["modifiedPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_x);
-        datafields["modifiedPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_y);
-        datafields["modifiedPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_z);
-        datafields["currentPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
-        datafields["currentPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
-        datafields["currentPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
-
-
-        datafields["modifiedPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_x);
-        datafields["modifiedPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_y);
-        datafields["modifiedPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_z);
-
-        datafields["currentPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
-        datafields["currentPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
-        datafields["currentPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
-        datafields["dmpBoxPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
-        datafields["dmpBoxPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
-        datafields["dmpBoxPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
-        datafields["dmpTwist_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_x);
-        datafields["dmpTwist_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_y);
-        datafields["dmpTwist_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_z);
-
-        datafields["modifiedTwist_lx"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lx);
-        datafields["modifiedTwist_ly"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ly);
-        datafields["modifiedTwist_lz"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lz);
-        datafields["modifiedTwist_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rx);
-        datafields["modifiedTwist_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ry);
-        datafields["modifiedTwist_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rz);
-
-        datafields["rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().rx);
-        datafields["ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().ry);
-        datafields["rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().rz);
-
-
-        debugObs->setDebugChannel("BimanualForceController", datafields);
-    }
-
-    void NJointBimanualForceController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        runTask("NJointBimanualForceController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-    }
-
-    void NJointBimanualForceController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-    }
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h
deleted file mode 100644
index 80a97c1fe3b925741945ff14e2d643d3e87060b5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h
+++ /dev/null
@@ -1,232 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualForceController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
-    TYPEDEF_PTRS_HANDLE(NJointBimanualForceController);
-    TYPEDEF_PTRS_HANDLE(NJointBimanualForceControlData);
-
-    class NJointBimanualForceControlData
-    {
-    public:
-        // from dmp
-        Eigen::Matrix4f boxPose;
-        Eigen::VectorXf boxTwist;
-        //        Eigen::VectorXf leftTargetTwist;
-        //        Eigen::VectorXf rightTargetTwist;
-
-        //        Eigen::Matrix4f leftTargetPose;
-        //        Eigen::Matrix4f rightTargetPose;
-
-        //        std::vector<float> nullspaceJointVelocities;
-        //        double virtualTime;
-    };
-
-
-    class NJointBimanualForceController :
-        public NJointControllerWithTripleBuffer<NJointBimanualForceControlData>,
-        public NJointBimanualForceControllerInterface
-    {
-    public:
-        //        using ConfigPtrT = BimanualForceControllerConfigPtr;
-        NJointBimanualForceController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointCCDMPControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
-        bool isFinished(const Ice::Current&)
-        {
-            return finished;
-        }
-
-        //        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
-        void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
-        double getVirtualTime(const Ice::Current&)
-        {
-            return virtualtimer;
-        }
-
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitNJointController();
-        void onDisconnectNJointController();
-        void controllerRun();
-
-    private:
-        Eigen::VectorXf targetWrench;
-        struct DebugBufferData
-        {
-            StringFloatDictionary desired_torques;
-
-            float modifiedPoseRight_x;
-            float modifiedPoseRight_y;
-            float modifiedPoseRight_z;
-            float currentPoseLeft_x;
-            float currentPoseLeft_y;
-            float currentPoseLeft_z;
-
-            float modifiedPoseLeft_x;
-            float modifiedPoseLeft_y;
-            float modifiedPoseLeft_z;
-            float currentPoseRight_x;
-            float currentPoseRight_y;
-            float currentPoseRight_z;
-
-            float dmpBoxPose_x;
-            float dmpBoxPose_y;
-            float dmpBoxPose_z;
-
-            float dmpTwist_x;
-            float dmpTwist_y;
-            float dmpTwist_z;
-
-            float modifiedTwist_lx;
-            float modifiedTwist_ly;
-            float modifiedTwist_lz;
-            float modifiedTwist_rx;
-            float modifiedTwist_ry;
-            float modifiedTwist_rz;
-
-            float rx;
-            float ry;
-            float rz;
-
-            Eigen::VectorXf wrenchDMP;
-            Eigen::VectorXf computedBoxWrench;
-
-            Eigen::VectorXf forceImpedance;
-            Eigen::VectorXf forcePID;
-            Eigen::VectorXf forcePIDControlValue;
-            Eigen::VectorXf poseError;
-            Eigen::VectorXf wrenchesConstrained;
-            Eigen::VectorXf wrenchesMeasuredInRoot;
-        };
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-        struct NJointBimanualForceControllerSensorData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-        };
-        TripleBuffer<NJointBimanualForceControllerSensorData> controllerSensorData;
-
-        struct NJointBimanualForceControllerInterfaceData
-        {
-            Eigen::Matrix4f currentLeftPose;
-            Eigen::Matrix4f currentRightPose;
-        };
-
-        TripleBuffer<NJointBimanualForceControllerInterfaceData> interfaceData;
-
-        std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
-        std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
-        const SensorValueForceTorque* rightForceTorque;
-        const SensorValueForceTorque* leftForceTorque;
-
-        NJointBimanualForceControllerConfigPtr cfg;
-        VirtualRobot::DifferentialIKPtr leftIK;
-        VirtualRobot::DifferentialIKPtr rightIK;
-
-        TaskSpaceDMPControllerPtr objectDMP;
-
-
-
-        double virtualtimer;
-
-        mutable MutexType controllerMutex;
-        Eigen::VectorXf leftDesiredJointValues;
-        Eigen::VectorXf rightDesiredJointValues;
-
-        Eigen::Matrix4f leftInitialPose;
-        Eigen::Matrix4f rightInitialPose;
-        Eigen::Matrix4f boxInitialPose;
-
-        Eigen::VectorXf KpImpedance;
-        Eigen::VectorXf KdImpedance;
-        Eigen::VectorXf KpAdmittance;
-        Eigen::VectorXf KdAdmittance;
-        Eigen::VectorXf KmAdmittance;
-        Eigen::VectorXf KmPID;
-
-        Eigen::VectorXf modifiedAcc;
-        Eigen::VectorXf modifiedTwist;
-        Eigen::Matrix4f modifiedLeftPose;
-        Eigen::Matrix4f modifiedRightPose;
-
-        Eigen::Matrix4f sensorFrame2TcpFrameLeft;
-        Eigen::Matrix4f sensorFrame2TcpFrameRight;
-
-        //static compensation
-        float massLeft;
-        Eigen::Vector3f CoMVecLeft;
-        Eigen::Vector3f forceOffsetLeft;
-        Eigen::Vector3f torqueOffsetLeft;
-
-        float massRight;
-        Eigen::Vector3f CoMVecRight;
-        Eigen::Vector3f forceOffsetRight;
-        Eigen::Vector3f torqueOffsetRight;
-
-
-
-        //        float knull;
-        //        float dnull;
-
-        std::vector<std::string> leftJointNames;
-        std::vector<std::string> rightJointNames;
-
-        //        float torqueLimit;
-        VirtualRobot::RobotNodeSetPtr leftRNS;
-        VirtualRobot::RobotNodeSetPtr rightRNS;
-        VirtualRobot::RobotNodePtr tcpLeft;
-        VirtualRobot::RobotNodePtr tcpRight;
-
-        std::vector<PIDControllerPtr> forcePIDControllers;
-
-        // filter parameters
-        float filterCoeff;
-        Eigen::VectorXf filteredOldValue;
-        bool finished;
-        bool dmpStarted;
-    protected:
-        //        void rtPreActivateController();
-        bool firstLoop;
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
deleted file mode 100644
index 74e8c8dceb83a53134ff155e772fcad08b3bec1f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ /dev/null
@@ -1,1140 +0,0 @@
-#include "NJointBimanualObjLevelController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointBimanualObjLevelController> registrationControllerNJointBimanualObjLevelController("NJointBimanualObjLevelController");
-
-    NJointBimanualObjLevelController::NJointBimanualObjLevelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "Initializing Bimanual Object Level Controller";
-
-        useSynchronizedRtRobot();
-        cfg = NJointBimanualObjLevelControllerConfigPtr::dynamicCast(config);
-        //        ARMARX_CHECK_EXPRESSION(prov);
-        //        RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
-        //        ARMARX_CHECK_EXPRESSION(robotUnit);
-        leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-
-        for (size_t i = 0; i < leftRNS->getSize(); ++i)
-        {
-            std::string jointName = leftRNS->getNode(i)->getName();
-            leftJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-
-            leftVelocitySensors.push_back(velocitySensor);
-            leftPositionSensors.push_back(positionSensor);
-
-        };
-
-        rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
-        for (size_t i = 0; i < rightRNS->getSize(); ++i)
-        {
-            std::string jointName = rightRNS->getNode(i)->getName();
-            rightJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-            rightVelocitySensors.push_back(velocitySensor);
-            rightPositionSensors.push_back(positionSensor);
-
-        };
-
-
-        //        const SensorValueBase* svlf = prov->getSensorValue("FT L");
-        const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
-        leftForceTorque = svlf->asA<SensorValueForceTorque>();
-        //        const SensorValueBase* svrf = prov->getSensorValue("FT R");
-        const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
-        rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
-        leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
-        taskSpaceDMPConfig.DMPAmplitude = 1.0;
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-
-
-        objectDMP.reset(new TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
-        ARMARX_IMPORTANT << "dmp finieshed";
-
-        tcpLeft = leftRNS->getTCP();
-        tcpRight = rightRNS->getTCP();
-
-        //initialize control parameters
-        KpImpedance.setZero(cfg->KpImpedance.size());
-        ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 12);
-
-        for (int i = 0; i < KpImpedance.size(); ++i)
-        {
-            KpImpedance(i) = cfg->KpImpedance.at(i);
-        }
-
-        KdImpedance.setZero(cfg->KdImpedance.size());
-        ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 12);
-
-        for (int i = 0; i < KdImpedance.size(); ++i)
-        {
-            KdImpedance(i) = cfg->KdImpedance.at(i);
-        }
-
-        KpAdmittance.setZero(cfg->KpAdmittance.size());
-        ARMARX_CHECK_EQUAL(cfg->KpAdmittance.size(), 6);
-
-        for (int i = 0; i < KpAdmittance.size(); ++i)
-        {
-            KpAdmittance(i) = cfg->KpAdmittance.at(i);
-        }
-
-        KdAdmittance.setZero(cfg->KdAdmittance.size());
-        ARMARX_CHECK_EQUAL(cfg->KdAdmittance.size(), 6);
-
-        for (int i = 0; i < KdAdmittance.size(); ++i)
-        {
-            KdAdmittance(i) = cfg->KdAdmittance.at(i);
-        }
-
-        KmAdmittance.setZero(cfg->KmAdmittance.size());
-        ARMARX_CHECK_EQUAL(cfg->KmAdmittance.size(), 6);
-
-        for (int i = 0; i < KmAdmittance.size(); ++i)
-        {
-            KmAdmittance(i) = cfg->KmAdmittance.at(i);
-        }
-
-
-        Inferface2rtData initInt2rtData;
-        initInt2rtData.KpImpedance = KpImpedance;
-        initInt2rtData.KdImpedance = KdImpedance;
-        initInt2rtData.KmAdmittance = KmAdmittance;
-        initInt2rtData.KpAdmittance = KpAdmittance;
-        initInt2rtData.KdAdmittance = KdAdmittance;
-        interface2rtBuffer.reinitAllBuffers(initInt2rtData);
-
-        leftDesiredJointValues.resize(leftTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
-        }
-
-        rightDesiredJointValues.resize(rightTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
-        }
-
-        KmPID.resize(cfg->KmPID.size());
-        ARMARX_CHECK_EQUAL(cfg->KmPID.size(), 6);
-
-        for (int i = 0; i < KmPID.size(); ++i)
-        {
-            KmPID(i) = cfg->KmPID.at(i);
-        }
-
-        virtualAcc.setZero(6);
-        virtualVel.setZero(6);
-        virtualPose = Eigen::Matrix4f::Identity();
-        ARMARX_INFO << "got controller params";
-
-
-        boxInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->boxInitialPose[4], cfg->boxInitialPose[5], cfg->boxInitialPose[6], cfg->boxInitialPose[3]);
-        for (int i = 0; i < 3; ++i)
-        {
-            boxInitialPose(i, 3) = cfg->boxInitialPose[i];
-        }
-
-        rt2ControlData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentPose = boxInitialPose;
-        initSensorData.currentTwist.setZero();
-        rt2ControlBuffer.reinitAllBuffers(initSensorData);
-
-
-        ControlInterfaceData initInterfaceData;
-        initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
-        initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
-        initInterfaceData.currentObjPose = boxInitialPose;
-        initInterfaceData.currentObjForce.setZero();
-        initInterfaceData.currentObjVel.setZero();
-        controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
-
-
-        leftInitialPose = tcpLeft->getPoseInRootFrame();
-        rightInitialPose = tcpRight->getPoseInRootFrame();
-        leftInitialPose.block<3, 1>(0, 3) = 0.001 * leftInitialPose.block<3, 1>(0, 3);
-        rightInitialPose.block<3, 1>(0, 3) = 0.001 * rightInitialPose.block<3, 1>(0, 3);
-
-        //        leftInitialPose = boxInitialPose;
-        //        leftInitialPose(0, 3) -= cfg->boxWidth * 0.5;
-        //        rightInitialPose = boxInitialPose;
-        //        rightInitialPose(0, 3) += cfg->boxWidth * 0.5;
-
-        forcePIDControllers.resize(12);
-        for (size_t i = 0; i < 6; i++)
-        {
-            forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
-            forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
-            forcePIDControllers.at(i)->reset();
-            forcePIDControllers.at(i + 6)->reset();
-        }
-
-        // filter
-        filterCoeff = cfg->filterCoeff;
-        ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
-        filteredOldValue.setZero(12);
-
-        // static compensation
-        massLeft = cfg->massLeft;
-        CoMVecLeft << cfg->CoMVecLeft[0],  cfg->CoMVecLeft[1],  cfg->CoMVecLeft[2];
-        forceOffsetLeft << cfg->forceOffsetLeft[0],  cfg->forceOffsetLeft[1],  cfg->forceOffsetLeft[2];
-        torqueOffsetLeft << cfg->torqueOffsetLeft[0],  cfg->torqueOffsetLeft[1],  cfg->torqueOffsetLeft[2];
-
-        massRight = cfg->massRight;
-        CoMVecRight << cfg->CoMVecRight[0],  cfg->CoMVecRight[1],  cfg->CoMVecRight[2];
-        forceOffsetRight << cfg->forceOffsetRight[0],  cfg->forceOffsetRight[1],  cfg->forceOffsetRight[2];
-        torqueOffsetRight << cfg->torqueOffsetRight[0],  cfg->torqueOffsetRight[1],  cfg->torqueOffsetRight[2];
-
-        sensorFrame2TcpFrameLeft.setZero();
-        sensorFrame2TcpFrameRight.setZero();
-
-        NJointBimanualObjLevelControlData initData;
-        initData.boxPose = boxInitialPose;
-        initData.boxTwist.setZero(6);
-        reinitTripleBuffer(initData);
-
-        firstLoop = true;
-        ARMARX_INFO << "left initial pose: \n" << leftInitialPose  << "\n right initial pose: \n" << rightInitialPose;
-
-        ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
-        ARMARX_IMPORTANT << "finished construction!";
-
-        dmpStarted = false;
-
-
-        ftcalibrationTimer = 0;
-        ftOffset.setZero(12);
-
-        targetWrench.setZero(cfg->targetWrench.size());
-        for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
-        {
-            targetWrench(i) = cfg->targetWrench[i];
-        }
-
-
-        fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
-        objCom2TCPLeftInObjFrame.setZero();
-        objCom2TCPRightInObjFrame.setZero();
-
-    }
-
-    void NJointBimanualObjLevelController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
-    {
-        objectDMP->setWeights(weights);
-    }
-
-    DoubleSeqSeq NJointBimanualObjLevelController::getMPWeights(const Ice::Current&)
-    {
-        DMP::DVec2d res = objectDMP->getWeights();
-        DoubleSeqSeq resvec;
-        for (size_t i = 0; i < res.size(); ++i)
-        {
-            std::vector<double> cvec;
-            for (size_t j = 0; j < res[i].size(); ++j)
-            {
-                cvec.push_back(res[i][j]);
-            }
-            resvec.push_back(cvec);
-        }
-
-        return resvec;
-    }
-
-    void NJointBimanualObjLevelController::setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&)
-    {
-        objectDMP->setRotWeights(weights);
-    }
-
-    DoubleSeqSeq NJointBimanualObjLevelController::getMPRotWeights(const Ice::Current&)
-    {
-        DMP::DVec2d res = objectDMP->getRotWeights();
-        DoubleSeqSeq resvec;
-        for (size_t i = 0; i < res.size(); ++i)
-        {
-            std::vector<double> cvec;
-            for (size_t j = 0; j < res[i].size(); ++j)
-            {
-                cvec.push_back(res[i][j]);
-            }
-            resvec.push_back(cvec);
-        }
-
-        return resvec;
-    }
-
-    void NJointBimanualObjLevelController::rtPreActivateController()
-    {
-        Eigen::Matrix4f boxInitPose = Eigen::Matrix4f::Identity();
-        Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
-        leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) * 0.001;
-        rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) * 0.001;
-        boxInitPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
-        boxInitPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
-
-        NJointBimanualObjLevelControlData initData;
-        initData.boxPose = boxInitPose;
-        initData.boxTwist.resize(6);
-        reinitTripleBuffer(initData);
-    }
-
-    std::string NJointBimanualObjLevelController::getClassName(const Ice::Current&) const
-    {
-        return "NJointBimanualObjLevelController";
-    }
-
-    void NJointBimanualObjLevelController::controllerRun()
-    {
-        if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
-        {
-            return;
-        }
-
-        double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
-        Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
-        Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
-        //ARMARX_IMPORTANT << "canVal:  " << objectDMP->canVal;
-
-        if (objectDMP->canVal < 1e-8)
-        {
-            finished = true;
-            dmpStarted = false;
-        }
-
-        objectDMP->flow(deltaT, currentPose, currentTwist);
-
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
-        getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
-        writeControlStruct();
-    }
-
-
-
-
-    void NJointBimanualObjLevelController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
-
-        controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
-        controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-        ftcalibrationTimer += deltaT;
-
-        if (firstLoop)
-        {
-            Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
-            Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
-
-            leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) * 0.001;
-            rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) * 0.001;
-
-            virtualPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
-            virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
-            fixedLeftRightRotOffset =  virtualPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
-
-            Eigen::Vector3f objCom2TCPLeftInGlobal = leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-            Eigen::Vector3f objCom2TCPRightInGlobal = rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-            //        objCom2TCPLeft << -cfg->boxWidth * 0.5, 0, 0;
-            //        objCom2TCPRight << cfg->boxWidth * 0.5, 0, 0;
-
-            objCom2TCPLeftInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
-            objCom2TCPRightInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
-
-
-            Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
-            Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
-            leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
-            rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
-
-            sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
-            sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
-            CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
-            CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
-            firstLoop = false;
-            ARMARX_INFO << "modified left pose:\n " << leftPose;
-            ARMARX_INFO << "modified right pose:\n " << rightPose;
-        }
-
-        // --------------------------------------------- get control parameters ---------------------------------------
-        KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
-        KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
-        KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
-        KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
-        KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
-
-        if (ftcalibrationTimer < cfg->ftCalibrationTime)
-        {
-            ftOffset.block<3, 1>(0, 0) = 0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
-            ftOffset.block<3, 1>(3, 0) = 0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
-            ftOffset.block<3, 1>(6, 0) = 0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
-            ftOffset.block<3, 1>(9, 0) = 0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
-
-            for (int i = 0; i < KmAdmittance.size(); ++i)
-            {
-                KmAdmittance(i) = 0;
-            }
-        }
-        else
-        {
-            for (int i = 0; i < KmAdmittance.size(); ++i)
-            {
-                KmAdmittance(i) = cfg->KmAdmittance.at(i);
-            }
-        }
-
-        // -------------------------------------------- target wrench ---------------------------------------------
-        Eigen::VectorXf deltaPoseForWrenchControl;
-        deltaPoseForWrenchControl.setZero(12);
-        for (size_t i = 0; i < 12; ++i)
-        {
-            if (KpImpedance(i) == 0)
-            {
-                deltaPoseForWrenchControl(i) = 0;
-            }
-            else
-            {
-                deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
-                if (deltaPoseForWrenchControl(i) > 0.1)
-                {
-                    deltaPoseForWrenchControl(i) = 0.1;
-                }
-                else if (deltaPoseForWrenchControl(i) < -0.1)
-                {
-                    deltaPoseForWrenchControl(i) = -0.1;
-                }
-                //            deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
-            }
-        }
-
-        // ------------------------------------------- current tcp pose -------------------------------------------
-
-        currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
-        currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
-
-        // --------------------------------------------- grasp matrix ---------------------------------------------
-
-        Eigen::MatrixXf graspMatrix;
-        graspMatrix.setZero(6, 12);
-        graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
-        graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
-        //        graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
-        //        graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
-
-        Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
-        graspMatrix.block<3, 3>(3, 0) = skew(rvec);
-
-        rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
-        graspMatrix.block<3, 3>(3, 6) = skew(rvec);
-
-        // // projection of grasp matrix
-        // Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
-        // Eigen::MatrixXf G_range = pinvG * graspMatrix;
-        // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
-        float lambda = 1;
-        Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
-
-        // ---------------------------------------------- object pose ----------------------------------------------
-        Eigen::Matrix4f boxCurrentPose = currentRightPose;
-        boxCurrentPose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
-        Eigen::VectorXf boxCurrentTwist;
-        boxCurrentTwist.setZero(6);
-
-        // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-        // Jacobian matrices
-        Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-        Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-        jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
-        jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
-        // qpos, qvel
-        Eigen::VectorXf leftqpos;
-        Eigen::VectorXf leftqvel;
-        leftqpos.resize(leftPositionSensors.size());
-        leftqvel.resize(leftVelocitySensors.size());
-        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
-        {
-            leftqpos(i) = leftPositionSensors[i]->position;
-            leftqvel(i) = leftVelocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf rightqpos;
-        Eigen::VectorXf rightqvel;
-        rightqpos.resize(rightPositionSensors.size());
-        rightqvel.resize(rightVelocitySensors.size());
-
-        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
-        {
-            rightqpos(i) = rightPositionSensors[i]->position;
-            rightqvel(i) = rightVelocitySensors[i]->velocity;
-        }
-
-        // -------------------------------------- compute TCP and object velocity -------------------------------------
-        Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
-        Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
-        Eigen::VectorXf currentTwist(12);
-        currentTwist << currentLeftTwist, currentRightTwist;
-        boxCurrentTwist = pinvGraspMatrixT * currentTwist;
-
-        rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
-        rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
-        rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
-        rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
-        rt2ControlBuffer.commitWrite();
-
-
-
-        // --------------------------------------------- get ft sensor ---------------------------------------------
-        // static compensation
-        Eigen::Vector3f gravity;
-        gravity << 0.0, 0.0, -9.8;
-        Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
-        Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
-        Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
-
-        Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
-        Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
-        Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
-
-        // mapping of measured wrenches
-        Eigen::VectorXf wrenchesMeasured(12);
-        wrenchesMeasured << leftForceTorque->force - forceOffsetLeft, leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight, rightForceTorque->torque - torqueOffsetRight;
-        for (size_t i = 0; i < 12; i++)
-        {
-            wrenchesMeasured(i) = (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
-        }
-        filteredOldValue = wrenchesMeasured;
-        wrenchesMeasured.block<3, 1>(0, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
-        wrenchesMeasured.block<3, 1>(3, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
-        wrenchesMeasured.block<3, 1>(6, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
-        wrenchesMeasured.block<3, 1>(9, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
-        //        if (wrenchesMeasured.norm() < cfg->forceThreshold)
-        //        {
-        //            wrenchesMeasured.setZero();
-        //        }
-
-        Eigen::VectorXf wrenchesMeasuredInRoot(12);
-        wrenchesMeasuredInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
-        wrenchesMeasuredInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
-        wrenchesMeasuredInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
-        wrenchesMeasuredInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
-
-
-        // map to object
-        Eigen::VectorXf objFTValue = graspMatrix * wrenchesMeasuredInRoot;
-        for (size_t i = 0; i < 6; i++)
-        {
-            if (fabs(objFTValue(i)) < cfg->forceThreshold.at(i))
-            {
-                objFTValue(i) = 0;
-            }
-            else
-            {
-                objFTValue(i) -= cfg->forceThreshold.at(i) * objFTValue(i) / fabs(objFTValue(i));
-            }
-        }
-
-        // pass sensor value to statechart
-        controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
-        controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head(3);
-        controlInterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head(3);
-        controlInterfaceBuffer.commitWrite();
-
-
-        // --------------------------------------------- get MP target ---------------------------------------------
-        Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
-        Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
-
-
-        // --------------------------------------------- obj admittance control ---------------------------------------------
-        // admittance
-        Eigen::VectorXf objPoseError(6);
-        objPoseError.head(3) = virtualPose.block<3, 1>(0, 3) - boxPose.block<3, 1>(0, 3);
-        Eigen::Matrix3f objDiffMat = virtualPose.block<3, 3>(0, 0) * boxPose.block<3, 3>(0, 0).transpose();
-        objPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
-
-
-        Eigen::VectorXf objAcc;
-        Eigen::VectorXf objVel;
-        objAcc.setZero(6);
-        objVel.setZero(6);
-        for (size_t i = 0; i < 6; i++)
-        {
-            //            objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * (virtualVel(i) - boxTwist(i));
-            objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * virtualVel(i);
-        }
-        objVel = virtualVel + 0.5 * deltaT * (objAcc + virtualAcc);
-        Eigen::VectorXf deltaObjPose = 0.5 * deltaT * (objVel + virtualVel);
-        virtualAcc = objAcc;
-        virtualVel = objVel;
-        virtualPose.block<3, 1>(0, 3) += deltaObjPose.head(3);
-        virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) * virtualPose.block<3, 3>(0, 0);
-
-        // --------------------------------------------- convert to tcp pose ---------------------------------------------
-        Eigen::Matrix4f tcpTargetPoseLeft = virtualPose;
-        Eigen::Matrix4f tcpTargetPoseRight = virtualPose;
-
-        tcpTargetPoseRight.block<3, 3>(0, 0) = virtualPose.block<3, 3>(0, 0) * fixedLeftRightRotOffset;
-
-        tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
-        tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
-
-        // --------------------------------------------- Impedance control ---------------------------------------------
-        Eigen::VectorXf poseError(12);
-        Eigen::Matrix3f diffMat = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(0, 0) = tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
-        poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        diffMat = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(6, 0) = tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
-        poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        Eigen::VectorXf forceImpedance(12);
-        for (size_t i = 0; i < 12; i++)
-        {
-            forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
-            //            forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
-        }
-
-        // --------------------------------------------- Nullspace control ---------------------------------------------
-        Eigen::VectorXf leftNullspaceTorque = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
-        Eigen::VectorXf rightNullspaceTorque = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
-
-        // --------------------------------------------- Set Torque Control Command ---------------------------------------------
-        //        float lambda = 1;
-        Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
-        Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
-        Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceImpedance.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
-        Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceImpedance.tail(6) + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
-        // torque limit
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            float desiredTorque =   leftJointDesiredTorques(i);
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-            desiredTorque = (desiredTorque >  cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
-            debugOutputData.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
-            leftTargets.at(i)->torque = desiredTorque;
-        }
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            float desiredTorque = rightJointDesiredTorques(i);
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-            desiredTorque = (desiredTorque >   cfg->torqueLimit) ?  cfg->torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < - cfg->torqueLimit) ? - cfg->torqueLimit : desiredTorque;
-            debugOutputData.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
-            rightTargets.at(i)->torque = desiredTorque;
-        }
-
-        // --------------------------------------------- debug output ---------------------------------------------
-        debugOutputData.getWriteBuffer().forceImpedance = forceImpedance;
-        debugOutputData.getWriteBuffer().poseError = poseError;
-        //        debugOutputData.getWriteBuffer().wrenchesConstrained = wrenchesConstrained;
-        debugOutputData.getWriteBuffer().wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
-        //        debugOutputData.getWriteBuffer().wrenchDMP = wrenchDMP;
-        //        debugOutputData.getWriteBuffer().computedBoxWrench = computedBoxWrench;
-
-        debugOutputData.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
-        debugOutputData.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
-        debugOutputData.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
-
-        debugOutputData.getWriteBuffer().objPose_x = boxCurrentPose(0, 3);
-        debugOutputData.getWriteBuffer().objPose_y = boxCurrentPose(1, 3);
-        debugOutputData.getWriteBuffer().objPose_z = boxCurrentPose(2, 3);
-
-        debugOutputData.getWriteBuffer().objForce_x = objFTValue(0);
-        debugOutputData.getWriteBuffer().objForce_y = objFTValue(1);
-        debugOutputData.getWriteBuffer().objForce_z = objFTValue(2);
-        debugOutputData.getWriteBuffer().objTorque_x = objFTValue(3);
-        debugOutputData.getWriteBuffer().objTorque_y = objFTValue(4);
-        debugOutputData.getWriteBuffer().objTorque_z = objFTValue(5);
-
-        debugOutputData.getWriteBuffer().objVel_x = objVel(0);
-        debugOutputData.getWriteBuffer().objVel_y = objVel(1);
-        debugOutputData.getWriteBuffer().objVel_z = objVel(2);
-        debugOutputData.getWriteBuffer().objVel_rx = objVel(3);
-        debugOutputData.getWriteBuffer().objVel_ry = objVel(4);
-        debugOutputData.getWriteBuffer().objVel_rz = objVel(5);
-
-        debugOutputData.getWriteBuffer().deltaPose_x = deltaObjPose(0);
-        debugOutputData.getWriteBuffer().deltaPose_y = deltaObjPose(1);
-        debugOutputData.getWriteBuffer().deltaPose_z = deltaObjPose(2);
-        debugOutputData.getWriteBuffer().deltaPose_rx = deltaObjPose(3);
-        debugOutputData.getWriteBuffer().deltaPose_ry = deltaObjPose(4);
-        debugOutputData.getWriteBuffer().deltaPose_rz = deltaObjPose(5);
-
-        debugOutputData.getWriteBuffer().modifiedPoseRight_x = tcpTargetPoseRight(0, 3);
-        debugOutputData.getWriteBuffer().modifiedPoseRight_y = tcpTargetPoseRight(1, 3);
-        debugOutputData.getWriteBuffer().modifiedPoseRight_z = tcpTargetPoseRight(2, 3);
-
-        debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
-
-
-        VirtualRobot::MathTools::Quaternion leftQuat = VirtualRobot::MathTools::eigen4f2quat(currentLeftPose);
-        debugOutputData.getWriteBuffer().leftQuat_w = leftQuat.w;
-        debugOutputData.getWriteBuffer().leftQuat_x = leftQuat.x;
-        debugOutputData.getWriteBuffer().leftQuat_y = leftQuat.y;
-        debugOutputData.getWriteBuffer().leftQuat_z = leftQuat.y;
-
-        debugOutputData.getWriteBuffer().modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
-        debugOutputData.getWriteBuffer().modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
-        debugOutputData.getWriteBuffer().modifiedPoseLeft_z = tcpTargetPoseLeft(2, 3);
-
-        debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
-
-        VirtualRobot::MathTools::Quaternion rightQuat = VirtualRobot::MathTools::eigen4f2quat(currentRightPose);
-        debugOutputData.getWriteBuffer().rightQuat_w = rightQuat.w;
-        debugOutputData.getWriteBuffer().rightQuat_x = rightQuat.x;
-        debugOutputData.getWriteBuffer().rightQuat_y = rightQuat.y;
-        debugOutputData.getWriteBuffer().rightQuat_z = rightQuat.y;
-
-
-        debugOutputData.getWriteBuffer().dmpBoxPose_x = boxPose(0, 3);
-        debugOutputData.getWriteBuffer().dmpBoxPose_y = boxPose(1, 3);
-        debugOutputData.getWriteBuffer().dmpBoxPose_z = boxPose(2, 3);
-
-        debugOutputData.getWriteBuffer().dmpTwist_x = boxTwist(0);
-        debugOutputData.getWriteBuffer().dmpTwist_y = boxTwist(1);
-        debugOutputData.getWriteBuffer().dmpTwist_z = boxTwist(2);
-        debugOutputData.getWriteBuffer().rx = rvec(0);
-        debugOutputData.getWriteBuffer().ry = rvec(1);
-        debugOutputData.getWriteBuffer().rz = rvec(2);
-
-        //        debugOutputData.getWriteBuffer().modifiedTwist_lx = twistDMP(0);
-        //        debugOutputData.getWriteBuffer().modifiedTwist_ly = twistDMP(1);
-        //        debugOutputData.getWriteBuffer().modifiedTwist_lz = twistDMP(2);
-        //        debugOutputData.getWriteBuffer().modifiedTwist_rx = twistDMP(6);
-        //        debugOutputData.getWriteBuffer().modifiedTwist_ry = twistDMP(7);
-        //        debugOutputData.getWriteBuffer().modifiedTwist_rz = twistDMP(8);
-
-        //        debugOutputData.getWriteBuffer().forcePID = forcePIDInRootForDebug;
-
-        debugOutputData.commitWrite();
-
-    }
-
-    void NJointBimanualObjLevelController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        objectDMP->learnDMPFromFiles(fileNames);
-    }
-
-
-    void NJointBimanualObjLevelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard(controllerMutex);
-        objectDMP->setGoalPoseVec(goals);
-
-    }
-
-
-    void NJointBimanualObjLevelController::runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
-    {
-        while (!controlInterfaceBuffer.updateReadBuffer())
-        {
-            usleep(1000);
-        }
-
-        Eigen::Matrix4f leftPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentLeftPose;
-        Eigen::Matrix4f rightPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentRightPose;
-
-        VirtualRobot::MathTools::Quaternion boxOri = VirtualRobot::MathTools::eigen4f2quat(leftPose);
-        Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
-
-        ARMARX_IMPORTANT << "runDMP: boxPosi: " << boxPosi;
-
-        std::vector<double> boxInitialPose;
-        for (size_t i = 0; i < 3; ++i)
-        {
-            boxInitialPose.push_back(boxPosi(i) * 0.001); //Important: mm -> m
-        }
-        boxInitialPose.push_back(boxOri.w);
-        boxInitialPose.push_back(boxOri.x);
-        boxInitialPose.push_back(boxOri.y);
-        boxInitialPose.push_back(boxOri.z);
-
-        objectDMP->prepareExecution(boxInitialPose, goals);
-        objectDMP->canVal = timeDuration;
-        objectDMP->config.motionTimeDuration = timeDuration;
-
-
-        finished = false;
-        dmpStarted = true;
-    }
-
-    void NJointBimanualObjLevelController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
-    {
-        while (!controlInterfaceBuffer.updateReadBuffer())
-        {
-            usleep(1000);
-        }
-        ARMARX_IMPORTANT << "obj level control: setup dmp ...";
-        objectDMP->prepareExecution(starts, goals);
-        objectDMP->canVal = timeDuration;
-        objectDMP->config.motionTimeDuration = timeDuration;
-
-        finished = false;
-        dmpStarted = true;
-
-        ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
-    }
-
-    void NJointBimanualObjLevelController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-        //        LockGuardType guard(controllerMutex);
-        ARMARX_INFO << "setting via points ";
-        objectDMP->setViaPose(u, viapoint);
-    }
-
-    void NJointBimanualObjLevelController::removeAllViaPoints(const Ice::Current&)
-    {
-        objectDMP->removeAllViaPoints();
-    }
-
-    void NJointBimanualObjLevelController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 12);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
-        interface2rtBuffer.commitWrite();
-
-    }
-
-    void NJointBimanualObjLevelController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 12);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-    void NJointBimanualObjLevelController::setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-    void NJointBimanualObjLevelController::setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-    std::vector<float> NJointBimanualObjLevelController::getCurrentObjVel(const Ice::Current&)
-    {
-        Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
-        std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
-        return tvelvec;
-    }
-
-    std::vector<float> NJointBimanualObjLevelController::getCurrentObjForce(const Ice::Current&)
-    {
-        Eigen::Vector3f fvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
-        std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
-        return fvelvec;
-    }
-
-    void NJointBimanualObjLevelController::setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-
-    void NJointBimanualObjLevelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        Eigen::VectorXf forceImpedance = debugOutputData.getUpToDateReadBuffer().forceImpedance;
-        for (int i = 0; i < forceImpedance.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "forceImpedance_" + ss.str();
-            datafields[data_name] = new Variant(forceImpedance(i));
-        }
-
-        Eigen::VectorXf forcePID = debugOutputData.getUpToDateReadBuffer().forcePID;
-        for (int i = 0; i < forcePID.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "forcePID_" + ss.str();
-            datafields[data_name] = new Variant(forcePID(i));
-        }
-
-
-        Eigen::VectorXf poseError = debugOutputData.getUpToDateReadBuffer().poseError;
-        for (int i = 0; i < poseError.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "poseError_" + ss.str();
-            datafields[data_name] = new Variant(poseError(i));
-        }
-
-        Eigen::VectorXf wrenchesConstrained = debugOutputData.getUpToDateReadBuffer().wrenchesConstrained;
-        for (int i = 0; i < wrenchesConstrained.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "wrenchesConstrained_" + ss.str();
-            datafields[data_name] = new Variant(wrenchesConstrained(i));
-        }
-
-        Eigen::VectorXf wrenchesMeasuredInRoot = debugOutputData.getUpToDateReadBuffer().wrenchesMeasuredInRoot;
-        for (int i = 0; i < wrenchesMeasuredInRoot.rows(); ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string data_name = "wrenchesMeasuredInRoot_" + ss.str();
-            datafields[data_name] = new Variant(wrenchesMeasuredInRoot(i));
-        }
-
-
-        //        Eigen::VectorXf wrenchDMP = debugOutputData.getUpToDateReadBuffer().wrenchDMP;
-        //        for (size_t i = 0; i < wrenchDMP.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "wrenchDMP_" + ss.str();
-        //            datafields[data_name] = new Variant(wrenchDMP(i));
-        //        }
-
-        //        Eigen::VectorXf computedBoxWrench = debugOutputData.getUpToDateReadBuffer().computedBoxWrench;
-        //        for (size_t i = 0; i < computedBoxWrench.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "computedBoxWrench_" + ss.str();
-        //            datafields[data_name] = new Variant(computedBoxWrench(i));
-        //        }
-
-
-        datafields["virtualPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_x);
-        datafields["virtualPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_y);
-        datafields["virtualPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_z);
-
-        datafields["objPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_x);
-        datafields["objPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_y);
-        datafields["objPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_z);
-
-        datafields["objForce_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_x);
-        datafields["objForce_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_y);
-        datafields["objForce_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_z);
-        datafields["objTorque_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_x);
-        datafields["objTorque_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_y);
-        datafields["objTorque_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_z);
-
-        datafields["objVel_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_x);
-        datafields["objVel_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_y);
-        datafields["objVel_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_z);
-        datafields["objVel_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_rx);
-        datafields["objVel_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_ry);
-        datafields["objVel_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_rz);
-
-        datafields["deltaPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_x);
-        datafields["deltaPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_y);
-        datafields["deltaPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_z);
-        datafields["deltaPose_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_rx);
-        datafields["deltaPose_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_ry);
-        datafields["deltaPose_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_rz);
-
-        datafields["modifiedPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_x);
-        datafields["modifiedPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_y);
-        datafields["modifiedPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_z);
-        datafields["currentPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
-        datafields["currentPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
-        datafields["currentPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
-
-        datafields["leftQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_w);
-        datafields["leftQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_x);
-        datafields["leftQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_y);
-        datafields["leftQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_z);
-        datafields["rightQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_w);
-        datafields["rightQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_x);
-        datafields["rightQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_y);
-        datafields["rightQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_z);
-
-        datafields["modifiedPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_x);
-        datafields["modifiedPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_y);
-        datafields["modifiedPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_z);
-
-        datafields["currentPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
-        datafields["currentPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
-        datafields["currentPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
-        datafields["dmpBoxPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
-        datafields["dmpBoxPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
-        datafields["dmpBoxPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
-        datafields["dmpTwist_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_x);
-        datafields["dmpTwist_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_y);
-        datafields["dmpTwist_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_z);
-
-        datafields["modifiedTwist_lx"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lx);
-        datafields["modifiedTwist_ly"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ly);
-        datafields["modifiedTwist_lz"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lz);
-        datafields["modifiedTwist_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rx);
-        datafields["modifiedTwist_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ry);
-        datafields["modifiedTwist_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rz);
-
-        datafields["rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().rx);
-        datafields["ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().ry);
-        datafields["rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().rz);
-
-
-        debugObs->setDebugChannel("BimanualForceController", datafields);
-    }
-
-    void NJointBimanualObjLevelController::onInitNJointController()
-    {
-
-
-        ARMARX_INFO << "init ...";
-        runTask("NJointBimanualObjLevelController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-    }
-
-    void NJointBimanualObjLevelController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-    }
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
deleted file mode 100644
index efb7929b59dcb85fb4dd58d5174c689f5fd55119..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
+++ /dev/null
@@ -1,309 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
-    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelController);
-    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelControlData);
-
-    class NJointBimanualObjLevelControlData
-    {
-    public:
-        // control target from Movement Primitives
-        Eigen::Matrix4f boxPose;
-        Eigen::VectorXf boxTwist;
-    };
-
-
-    class NJointBimanualObjLevelController :
-        public NJointControllerWithTripleBuffer<NJointBimanualObjLevelControlData>,
-        public NJointBimanualObjLevelControllerInterface
-    {
-    public:
-        //        using ConfigPtrT = BimanualForceControllerConfigPtr;
-        NJointBimanualObjLevelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointCCDMPControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
-        bool isFinished(const Ice::Current&)
-        {
-            return finished;
-        }
-        Eigen::Matrix3f skew(Eigen::Vector3f vec)
-        {
-            Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
-            mat(1, 2) = -vec(0);
-            mat(0, 2) = vec(1);
-            mat(0, 1) = -vec(2);
-            mat(2, 1) = vec(0);
-            mat(2, 0) = -vec(1);
-            mat(1, 0) = vec(2);
-            return mat;
-        }
-
-        //        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
-        void runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-        void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
-        void removeAllViaPoints(const Ice::Current&);
-
-        double getVirtualTime(const Ice::Current&)
-        {
-            return virtualtimer;
-        }
-
-        void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
-        void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
-        void setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
-        void setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
-        void setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
-
-        std::vector<float> getCurrentObjVel(const Ice::Current&);
-        std::vector<float> getCurrentObjForce(const Ice::Current&);
-
-        void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
-        DoubleSeqSeq getMPWeights(const Ice::Current&);
-
-        void setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&);
-        DoubleSeqSeq getMPRotWeights(const Ice::Current&);
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitNJointController();
-        void onDisconnectNJointController();
-        void controllerRun();
-
-    private:
-        Eigen::VectorXf targetWrench;
-        struct DebugBufferData
-        {
-            StringFloatDictionary desired_torques;
-
-            float virtualPose_x;
-            float virtualPose_y;
-            float virtualPose_z;
-
-            float objPose_x;
-            float objPose_y;
-            float objPose_z;
-
-            float objForce_x;
-            float objForce_y;
-            float objForce_z;
-            float objTorque_x;
-            float objTorque_y;
-            float objTorque_z;
-
-            float deltaPose_x;
-            float deltaPose_y;
-            float deltaPose_z;
-            float deltaPose_rx;
-            float deltaPose_ry;
-            float deltaPose_rz;
-
-            float objVel_x;
-            float objVel_y;
-            float objVel_z;
-            float objVel_rx;
-            float objVel_ry;
-            float objVel_rz;
-
-            float modifiedPoseRight_x;
-            float modifiedPoseRight_y;
-            float modifiedPoseRight_z;
-            float currentPoseLeft_x;
-            float currentPoseLeft_y;
-            float currentPoseLeft_z;
-            float leftQuat_w;
-            float leftQuat_x;
-            float leftQuat_y;
-            float leftQuat_z;
-            float rightQuat_w;
-            float rightQuat_x;
-            float rightQuat_y;
-            float rightQuat_z;
-
-            float modifiedPoseLeft_x;
-            float modifiedPoseLeft_y;
-            float modifiedPoseLeft_z;
-            float currentPoseRight_x;
-            float currentPoseRight_y;
-            float currentPoseRight_z;
-
-            float dmpBoxPose_x;
-            float dmpBoxPose_y;
-            float dmpBoxPose_z;
-
-            float dmpTwist_x;
-            float dmpTwist_y;
-            float dmpTwist_z;
-
-            float modifiedTwist_lx;
-            float modifiedTwist_ly;
-            float modifiedTwist_lz;
-            float modifiedTwist_rx;
-            float modifiedTwist_ry;
-            float modifiedTwist_rz;
-
-            float rx;
-            float ry;
-            float rz;
-
-            Eigen::VectorXf wrenchDMP;
-            Eigen::VectorXf computedBoxWrench;
-
-            Eigen::VectorXf forceImpedance;
-            Eigen::VectorXf forcePID;
-            Eigen::VectorXf forcePIDControlValue;
-            Eigen::VectorXf poseError;
-            Eigen::VectorXf wrenchesConstrained;
-            Eigen::VectorXf wrenchesMeasuredInRoot;
-        };
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-        struct rt2ControlData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-        };
-        TripleBuffer<rt2ControlData> rt2ControlBuffer;
-
-        struct ControlInterfaceData
-        {
-            Eigen::Matrix4f currentLeftPose;
-            Eigen::Matrix4f currentRightPose;
-            Eigen::Matrix4f currentObjPose;
-            Eigen::Vector3f currentObjVel;
-            Eigen::Vector3f currentObjForce;
-        };
-
-        TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
-
-        struct Inferface2rtData
-        {
-            Eigen::VectorXf KpImpedance;
-            Eigen::VectorXf KdImpedance;
-            Eigen::VectorXf KmAdmittance;
-            Eigen::VectorXf KpAdmittance;
-            Eigen::VectorXf KdAdmittance;
-        };
-        TripleBuffer<Inferface2rtData> interface2rtBuffer;
-
-
-
-        std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
-        std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
-        const SensorValueForceTorque* rightForceTorque;
-        const SensorValueForceTorque* leftForceTorque;
-
-        NJointBimanualObjLevelControllerConfigPtr cfg;
-        VirtualRobot::DifferentialIKPtr leftIK;
-        VirtualRobot::DifferentialIKPtr rightIK;
-
-        TaskSpaceDMPControllerPtr objectDMP;
-
-
-
-        double virtualtimer;
-
-        mutable MutexType controllerMutex;
-        mutable MutexType interfaceDataMutex;
-        Eigen::VectorXf leftDesiredJointValues;
-        Eigen::VectorXf rightDesiredJointValues;
-
-        Eigen::Matrix4f leftInitialPose;
-        Eigen::Matrix4f rightInitialPose;
-        Eigen::Matrix4f boxInitialPose;
-
-        Eigen::VectorXf KpImpedance;
-        Eigen::VectorXf KdImpedance;
-        Eigen::VectorXf KpAdmittance;
-        Eigen::VectorXf KdAdmittance;
-        Eigen::VectorXf KmAdmittance;
-        Eigen::VectorXf KmPID;
-
-        Eigen::VectorXf virtualAcc;
-        Eigen::VectorXf virtualVel;
-        Eigen::Matrix4f virtualPose;
-
-        Eigen::Matrix4f sensorFrame2TcpFrameLeft;
-        Eigen::Matrix4f sensorFrame2TcpFrameRight;
-
-        //static compensation
-        float massLeft;
-        Eigen::Vector3f CoMVecLeft;
-        Eigen::Vector3f forceOffsetLeft;
-        Eigen::Vector3f torqueOffsetLeft;
-
-        float massRight;
-        Eigen::Vector3f CoMVecRight;
-        Eigen::Vector3f forceOffsetRight;
-        Eigen::Vector3f torqueOffsetRight;
-
-        //        float knull;
-        //        float dnull;
-
-        std::vector<std::string> leftJointNames;
-        std::vector<std::string> rightJointNames;
-
-        //        float torqueLimit;
-        VirtualRobot::RobotNodeSetPtr leftRNS;
-        VirtualRobot::RobotNodeSetPtr rightRNS;
-        VirtualRobot::RobotNodePtr tcpLeft;
-        VirtualRobot::RobotNodePtr tcpRight;
-
-        std::vector<PIDControllerPtr> forcePIDControllers;
-
-        // filter parameters
-        float filterCoeff;
-        Eigen::VectorXf filteredOldValue;
-        bool finished;
-        bool dmpStarted;
-        double ftcalibrationTimer;
-        Eigen::VectorXf ftOffset;
-
-        Eigen::Matrix3f fixedLeftRightRotOffset;
-        Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
-
-    protected:
-        void rtPreActivateController();
-        bool firstLoop;
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
deleted file mode 100644
index 26a9107eaa18578a1974c4045e28c9f72fbbde70..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
+++ /dev/null
@@ -1,1305 +0,0 @@
-#include "NJointBimanualObjLevelMultiMPController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointBimanualObjLevelMultiMPController> registrationControllerNJointBimanualObjLevelMultiMPController("NJointBimanualObjLevelMultiMPController");
-
-    NJointBimanualObjLevelMultiMPController::NJointBimanualObjLevelMultiMPController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "Initializing Bimanual Object Level Controller";
-
-        // initialize robot kinematic chain and sensors
-        useSynchronizedRtRobot();
-        cfg = NJointBimanualObjLevelMultiMPControllerConfigPtr::dynamicCast(config);
-
-        leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-
-        for (size_t i = 0; i < leftRNS->getSize(); ++i)
-        {
-            std::string jointName = leftRNS->getNode(i)->getName();
-            leftJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-
-            leftVelocitySensors.push_back(velocitySensor);
-            leftPositionSensors.push_back(positionSensor);
-
-        };
-
-        rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
-        for (size_t i = 0; i < rightRNS->getSize(); ++i)
-        {
-            std::string jointName = rightRNS->getNode(i)->getName();
-            rightJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-            rightVelocitySensors.push_back(velocitySensor);
-            rightPositionSensors.push_back(positionSensor);
-
-        };
-
-
-        const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
-        leftForceTorque = svlf->asA<SensorValueForceTorque>();
-        const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
-        rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
-        leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-        tcpLeft = leftRNS->getTCP();
-        tcpRight = rightRNS->getTCP();
-
-        /* ----------------- initialize DMP ----------------- */
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpObjType;
-        objectDMP.reset(new TaskSpaceDMPController("objDMP", taskSpaceDMPConfig, false));
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpLeftType;
-        leftTCPInObjDMP.reset(new TaskSpaceDMPController("leftDMP", taskSpaceDMPConfig, false));
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpRightType;
-        rightTCPInObjDMP.reset(new TaskSpaceDMPController("rightDMP", taskSpaceDMPConfig, false));
-        ARMARX_IMPORTANT << "dmps initialized";
-
-        /* ----------------- initialize control parameters ----------------- */
-        auto setParamVec = [&](Eigen::VectorXf & param, ::Ice::FloatSeq & cfgParam, const size_t n)
-        {
-            param.setZero(n);
-            ARMARX_CHECK_EQUAL(cfgParam.size(), n);
-
-            for (size_t i = 0; i < n; ++i)
-            {
-                param(i) = cfgParam.at(i);
-            }
-        };
-
-        setParamVec(KpImpedance, cfg->KpImpedance, 12);
-        setParamVec(KdImpedance, cfg->KdImpedance, 12);
-        setParamVec(KpAdmittance, cfg->KpAdmittance, 6);
-        setParamVec(KdAdmittance, cfg->KdAdmittance, 6);
-        setParamVec(KmAdmittance, cfg->KmAdmittance, 6);
-        setParamVec(KmPID, cfg->KmPID, 6);
-
-        ARMARX_INFO << "got controller params";
-
-        /* ------------- initialize default joint values ------------------- */
-        leftDesiredJointValues.resize(leftTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
-        }
-
-        rightDesiredJointValues.resize(rightTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
-        }
-
-        /* ------------- obtain object initial poses ------------------- */
-        // object pose (position in meter)
-        objInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->objInitialPose[4], cfg->objInitialPose[5], cfg->objInitialPose[6], cfg->objInitialPose[3]);
-        for (int i = 0; i < 3; ++i)
-        {
-            objInitialPose(i, 3) = cfg->objInitialPose[i];
-        }
-
-        virtualAcc.setZero(6);
-        virtualVel.setZero(6);
-        virtualPose = objInitialPose;
-
-        objTransMatrixInAnchor = Eigen::Matrix4f::Identity();
-
-        // obtain left & right initial pose in the object frame (in meter)
-        leftInitialPose = Eigen::Matrix4f::Identity();
-        rightInitialPose = Eigen::Matrix4f::Identity();
-        Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
-        leftInitialPose.block<3, 3>(0, 0)   = objInitialPose.block<3, 3>(0, 0).transpose() * leftPose.block<3, 3>(0, 0);
-        leftInitialPose.block<3, 1>(0, 3)  = objInitialPose.block<3, 3>(0, 0).transpose() * (leftPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
-        rightInitialPose.block<3, 3>(0, 0)  = objInitialPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
-        rightInitialPose.block<3, 1>(0, 3) = objInitialPose.block<3, 3>(0, 0).transpose() * (rightPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
-
-        integratedPoseObj = virtualPose;
-        integratedPoseLeft = leftInitialPose;  // need to be represented in the object local frame.
-        integratedPoseRight = rightInitialPose;
-        /* ------------- initialize buffers ------------------- */
-        // interface to RT
-        Inferface2rtData interface2rtData;
-        interface2rtData.KpImpedance = KpImpedance;
-        interface2rtData.KdImpedance = KdImpedance;
-        interface2rtData.KmAdmittance = KmAdmittance;
-        interface2rtData.KpAdmittance = KpAdmittance;
-        interface2rtData.KdAdmittance = KdAdmittance;
-        interface2rtBuffer.reinitAllBuffers(interface2rtData);
-
-        // RT to DMP
-        RT2ControlData rt2ControlData;
-        rt2ControlData.deltaT = 0;
-        rt2ControlData.currentTime = 0;
-        rt2ControlData.currentPoseObj = objInitialPose;
-        rt2ControlData.currentTwistObj.setZero();
-        rt2ControlData.currentPoseObj = leftInitialPose;
-        rt2ControlData.currentTwistObj.setZero();
-        rt2ControlData.currentPoseObj = rightInitialPose;
-        rt2ControlData.currentTwistObj.setZero();
-        rt2ControlBuffer.reinitAllBuffers(rt2ControlData);
-
-        // DMP (or interface) to RT Target
-        NJointBimanualObjLevelMultiMPControlData initData;
-        initData.objTargetPose = objInitialPose;
-        initData.objTargetTwist.setZero(6);
-
-        initData.leftTargetPoseInObj = leftInitialPose;
-        initData.leftTargetTwistInObj.setZero(6);
-
-        initData.rightTargetPoseInObj = rightInitialPose;
-        initData.rightTargetTwistInObj.setZero(6);
-        reinitTripleBuffer(initData);
-
-        // Control interface: RT to interface
-        RT2InterfaceData rt2InterfaceData;
-        rt2InterfaceData.currentLeftPoseInObjFrame = leftInitialPose;
-        rt2InterfaceData.currentRightPoseInObjFrame = rightInitialPose;
-        rt2InterfaceData.currentObjPose = objInitialPose;
-        rt2InterfaceData.currentObjForce.setZero();
-        rt2InterfaceData.currentObjVel.setZero();
-        rt2InterfaceBuffer.reinitAllBuffers(rt2InterfaceData);
-
-
-        /* ------------- initialize PID force controller ------------------- */
-        forcePIDControllers.resize(12);
-        for (size_t i = 0; i < 6; i++)
-        {
-            forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
-            forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
-            forcePIDControllers.at(i)->reset();
-            forcePIDControllers.at(i + 6)->reset();
-        }
-
-        /* ------------- initialize force torque sensor related ------------------- */
-        // sensor frame to tcp frame transformation
-        sensorFrame2TcpFrameLeft.setZero();
-        sensorFrame2TcpFrameRight.setZero();
-
-        // filter
-        filterCoeff = cfg->filterCoeff;
-        filteredOldValue.setZero(12);
-
-        // first loop calibrate
-        ftcalibrationTimer = 0;
-        ftOffset.setZero(12);
-
-        // target wrench
-        targetWrench.setZero(cfg->targetWrench.size());
-        for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
-        {
-            targetWrench(i) = cfg->targetWrench[i];
-        }
-
-        /* ------------- static compensation ------------------- */
-        massLeft = cfg->massLeft;
-        CoMVecLeft << cfg->CoMVecLeft[0],  cfg->CoMVecLeft[1],  cfg->CoMVecLeft[2];
-        forceOffsetLeft << cfg->forceOffsetLeft[0],  cfg->forceOffsetLeft[1],  cfg->forceOffsetLeft[2];
-        torqueOffsetLeft << cfg->torqueOffsetLeft[0],  cfg->torqueOffsetLeft[1],  cfg->torqueOffsetLeft[2];
-
-        massRight = cfg->massRight;
-        CoMVecRight << cfg->CoMVecRight[0],  cfg->CoMVecRight[1],  cfg->CoMVecRight[2];
-        forceOffsetRight << cfg->forceOffsetRight[0],  cfg->forceOffsetRight[1],  cfg->forceOffsetRight[2];
-        torqueOffsetRight << cfg->torqueOffsetRight[0],  cfg->torqueOffsetRight[1],  cfg->torqueOffsetRight[2];
-
-        /* ------------- Pose Offset ------------------- */
-        fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
-        objCom2TCPLeftInObjFrame.setZero();
-        objCom2TCPRightInObjFrame.setZero();
-
-        /* ------------- setup flags ------------------- */
-        firstLoop = true;
-        dmpStarted = false;
-        ARMARX_IMPORTANT << "finished construction!";
-    }
-
-
-    void NJointBimanualObjLevelMultiMPController::rtPreActivateController()
-    {}
-
-    std::string NJointBimanualObjLevelMultiMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointBimanualObjLevelMultiMPController";
-    }
-
-    void NJointBimanualObjLevelMultiMPController::controllerRun()
-    {
-        if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
-        {
-            return;
-        }
-
-        // get status from RT loop
-        double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
-        Eigen::Matrix4f currentPoseObj          = rt2ControlBuffer.getReadBuffer().currentPoseObj;
-        Eigen::VectorXf currentTwistObj         = rt2ControlBuffer.getReadBuffer().currentTwistObj;
-        Eigen::Matrix4f currentPoseLeftInObj    = rt2ControlBuffer.getReadBuffer().currentPoseLeftInObj;
-        Eigen::VectorXf currentTwistLeftInObj   = rt2ControlBuffer.getReadBuffer().currentTwistLeftInObj;
-        Eigen::Matrix4f currentPoseRightInObj   = rt2ControlBuffer.getReadBuffer().currentPoseRightInObj;
-        Eigen::VectorXf currentTwistRightInObj  = rt2ControlBuffer.getReadBuffer().currentTwistRightInObj;
-
-        // set can val from outside
-        rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
-
-        // DMP flow control
-        bool objectDMPFinished = false;
-        bool leftTCPInObjDMPFinished = false;
-        bool rightTCPInObjDMPFinished = false;
-
-        if (objectDMP->canVal > 1e-8 || objectDMP->config.DMPStyle == "Periodic")
-        {
-            objectDMP->flow(deltaT, currentPoseObj, currentTwistObj);
-            getWriterControlStruct().objTargetPose = objectDMP->getTargetPoseMat();
-            getWriterControlStruct().objTargetTwist = objectDMP->getTargetVelocity();
-        }
-        else
-        {
-            objectDMPFinished = true;
-        }
-
-
-        if (leftTCPInObjDMP->canVal > 1e-8 || leftTCPInObjDMP->config.DMPStyle == "Periodic")
-        {
-            leftTCPInObjDMP->flow(deltaT, currentPoseLeftInObj, currentTwistLeftInObj);
-            getWriterControlStruct().leftTargetPoseInObj = leftTCPInObjDMP->getTargetPoseMat();
-            getWriterControlStruct().leftTargetTwistInObj = leftTCPInObjDMP->getTargetVelocity();
-        }
-        else
-        {
-            leftTCPInObjDMPFinished = true;
-        }
-
-        if (rightTCPInObjDMP->canVal > 1e-8 || rightTCPInObjDMP->config.DMPStyle == "Periodic")
-        {
-            rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
-            getWriterControlStruct().rightTargetPoseInObj = rightTCPInObjDMP->getTargetPoseMat();
-            getWriterControlStruct().rightTargetTwistInObj = rightTCPInObjDMP->getTargetVelocity();
-        }
-        else
-        {
-            rightTCPInObjDMPFinished = true;
-        }
-
-
-        if (objectDMPFinished && leftTCPInObjDMPFinished && rightTCPInObjDMPFinished)
-        {
-            finished = true;
-            dmpStarted = false;
-        }
-
-        // set target to RT loop
-        LockGuardType guard {controlDataMutex};
-        writeControlStruct();
-    }
-
-    void NJointBimanualObjLevelMultiMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        // ------------------------------------------- current tcp pose -------------------------------------------
-        Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
-
-        currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
-        currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
-
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-        ftcalibrationTimer += deltaT;
-
-        if (firstLoop)
-        {
-            Eigen::Matrix4f leftPose = currentLeftPose;
-            Eigen::Matrix4f rightPose = currentRightPose;
-
-            Eigen::Vector3f objCom2TCPLeftInGlobal = leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-            Eigen::Vector3f objCom2TCPRightInGlobal = rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-
-            // Temporary (until we could detect and set object pose by perception)
-            // T_obj = T_{leftHand} * T_{objTransMatrixInAnchor}
-            objTransMatrixInAnchor.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * objInitialPose.block<3, 3>(0, 0);
-            objTransMatrixInAnchor.block<3, 1>(0, 3) = leftPose.block<3, 3>(0, 0).transpose() * (objInitialPose.block<3, 1>(0, 3) - leftPose.block<3, 1>(0, 3));
-
-            objCom2TCPLeftInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
-            objCom2TCPRightInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
-
-
-            Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
-            Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
-            leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
-            rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
-
-            // sensor frame 2 tcp frame transformation
-            sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
-            sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
-            CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
-            CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
-            firstLoop = false;
-        }
-
-        Eigen::Matrix4f currentLeftPoseInObjFrame = Eigen::Matrix4f::Identity();
-        Eigen::Matrix4f currentRightPoseInObjFrame = Eigen::Matrix4f::Identity();
-        currentLeftPoseInObjFrame.block<3, 3>(0, 0)   = virtualPose.block<3, 3>(0, 0).transpose() * currentLeftPose.block<3, 3>(0, 0);
-        currentLeftPoseInObjFrame.block<3, 1>(0, 3)  = virtualPose.block<3, 3>(0, 0).transpose() * (currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
-        currentRightPoseInObjFrame.block<3, 3>(0, 0)  = virtualPose.block<3, 3>(0, 0).transpose() * currentRightPose.block<3, 3>(0, 0);
-        currentRightPoseInObjFrame.block<3, 1>(0, 3) = virtualPose.block<3, 3>(0, 0).transpose() * (currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
-        rt2InterfaceBuffer.getWriteBuffer().currentLeftPoseInObjFrame = currentLeftPoseInObjFrame;
-        rt2InterfaceBuffer.getWriteBuffer().currentRightPoseInObjFrame = currentRightPoseInObjFrame;
-
-
-        // --------------------------------------------- get control parameters ---------------------------------------
-        KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
-        KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
-        KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
-        KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
-        KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
-
-        if (ftcalibrationTimer < cfg->ftCalibrationTime)
-        {
-            ftOffset.block<3, 1>(0, 0) = 0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
-            ftOffset.block<3, 1>(3, 0) = 0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
-            ftOffset.block<3, 1>(6, 0) = 0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
-            ftOffset.block<3, 1>(9, 0) = 0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
-
-            for (int i = 0; i < KmAdmittance.size(); ++i)
-            {
-                KmAdmittance(i) = 0;
-            }
-        }
-        else
-        {
-            for (int i = 0; i < KmAdmittance.size(); ++i)
-            {
-                KmAdmittance(i) = cfg->KmAdmittance.at(i);
-            }
-        }
-
-        // -------------------------------------------- target wrench ---------------------------------------------
-        Eigen::VectorXf deltaPoseForWrenchControl;
-        deltaPoseForWrenchControl.setZero(12);
-        for (size_t i = 0; i < 12; ++i)
-        {
-            if (KpImpedance(i) == 0)
-            {
-                deltaPoseForWrenchControl(i) = 0;
-            }
-            else
-            {
-                deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
-                if (deltaPoseForWrenchControl(i) > 0.1)
-                {
-                    deltaPoseForWrenchControl(i) = 0.1;
-                }
-                else if (deltaPoseForWrenchControl(i) < -0.1)
-                {
-                    deltaPoseForWrenchControl(i) = -0.1;
-                }
-                //            deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
-            }
-        }
-
-
-        // --------------------------------------------- grasp matrix ---------------------------------------------
-
-        Eigen::Vector3f objCom2TCPLeftInGlobal = currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-        Eigen::Vector3f objCom2TCPRightInGlobal = currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-
-        Eigen::MatrixXf graspMatrix;
-        graspMatrix.setZero(6, 12);
-        graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
-        graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
-        //        graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
-        //        graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
-
-        //        Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
-        graspMatrix.block<3, 3>(3, 0) = skew(objCom2TCPLeftInGlobal);
-
-        //        rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
-        graspMatrix.block<3, 3>(3, 6) = skew(objCom2TCPRightInGlobal);
-
-        // // projection of grasp matrix
-        // Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
-        // Eigen::MatrixXf G_range = pinvG * graspMatrix;
-        // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
-        float lambda = 1;
-        Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
-
-        // ---------------------------------------------- object pose ----------------------------------------------
-        Eigen::Matrix4f objCurrentPose = currentLeftPose * objTransMatrixInAnchor;
-        Eigen::VectorXf objCurrentTwist;
-        //        getObjStatus(objCurrentPose, objCurrentTwist);
-        objCurrentTwist.setZero(6);
-
-        // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-        // Jacobian matrices
-        Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-        Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-        jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
-        jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
-        // qpos, qvel
-        Eigen::VectorXf leftqpos;
-        Eigen::VectorXf leftqvel;
-        leftqpos.resize(leftPositionSensors.size());
-        leftqvel.resize(leftVelocitySensors.size());
-        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
-        {
-            leftqpos(i) = leftPositionSensors[i]->position;
-            leftqvel(i) = leftVelocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf rightqpos;
-        Eigen::VectorXf rightqvel;
-        rightqpos.resize(rightPositionSensors.size());
-        rightqvel.resize(rightVelocitySensors.size());
-
-        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
-        {
-            rightqpos(i) = rightPositionSensors[i]->position;
-            rightqvel(i) = rightVelocitySensors[i]->velocity;
-        }
-
-        // -------------------------------------- compute TCP and object velocity -------------------------------------
-        Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
-        Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
-        Eigen::VectorXf currentTwist(12);
-        currentTwist << currentLeftTwist, currentRightTwist;
-        objCurrentTwist = pinvGraspMatrixT * currentTwist;
-
-        rt2ControlBuffer.getWriteBuffer().currentPoseObj = objCurrentPose;
-        rt2ControlBuffer.getWriteBuffer().currentTwistObj = objCurrentTwist;
-        rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
-        rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
-        rt2ControlBuffer.commitWrite();
-
-        // --------------------------------------------- get ft sensor ---------------------------------------------
-        // static compensation
-        Eigen::Vector3f gravity;
-        gravity << 0.0, 0.0, -9.8;
-        Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
-        Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
-        Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
-
-        Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
-        Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
-        Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
-
-        // mapping of measured wrenches
-        Eigen::VectorXf wrenchesMeasured(12);
-        wrenchesMeasured << leftForceTorque->force - forceOffsetLeft, leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight, rightForceTorque->torque - torqueOffsetRight;
-        for (size_t i = 0; i < 12; i++)
-        {
-            wrenchesMeasured(i) = (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
-        }
-        filteredOldValue = wrenchesMeasured;
-        wrenchesMeasured.block<3, 1>(0, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
-        wrenchesMeasured.block<3, 1>(3, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
-        wrenchesMeasured.block<3, 1>(6, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
-        wrenchesMeasured.block<3, 1>(9, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
-        //        if (wrenchesMeasured.norm() < cfg->forceThreshold)
-        //        {
-        //            wrenchesMeasured.setZero();
-        //        }
-
-        Eigen::VectorXf forceTorqueMeasurementInRoot(12);
-        forceTorqueMeasurementInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
-        forceTorqueMeasurementInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
-        forceTorqueMeasurementInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
-        forceTorqueMeasurementInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
-
-
-        // map to object
-        Eigen::VectorXf objFTValue = graspMatrix * forceTorqueMeasurementInRoot;
-        for (size_t i = 0; i < 6; i++)
-        {
-            if (fabs(objFTValue(i)) < cfg->forceThreshold.at(i))
-            {
-                objFTValue(i) = 0;
-            }
-            else
-            {
-                objFTValue(i) -= cfg->forceThreshold.at(i) * objFTValue(i) / fabs(objFTValue(i));
-            }
-        }
-
-        // pass sensor value to statechart
-        rt2InterfaceBuffer.getWriteBuffer().currentObjPose = objCurrentPose;
-        rt2InterfaceBuffer.getWriteBuffer().currentObjVel = objCurrentTwist.head(3);
-        rt2InterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head(3);
-        rt2InterfaceBuffer.commitWrite();
-
-
-        // --------------------------------------------- get MP target ---------------------------------------------
-        Eigen::Matrix4f objTargetPose = rtGetControlStruct().objTargetPose;
-        Eigen::VectorXf objTargetTwist = rtGetControlStruct().objTargetTwist;
-
-        Eigen::Matrix4f leftPoseInObj = rtGetControlStruct().leftTargetPoseInObj;
-        Eigen::VectorXf leftTwistInObj = rtGetControlStruct().leftTargetTwistInObj;
-
-        Eigen::Matrix4f rightPoseInObj = rtGetControlStruct().rightTargetPoseInObj;
-        Eigen::VectorXf rightTwistInObj = rtGetControlStruct().rightTargetTwistInObj;
-
-        // integrate mp target
-        integrateVel2Pose(deltaT, objTargetTwist, integratedPoseObj);
-        integrateVel2Pose(deltaT, leftTwistInObj, integratedPoseLeft);
-        integrateVel2Pose(deltaT, rightTwistInObj, integratedPoseRight);
-
-        // --------------------------------------------- obj admittance control ---------------------------------------------
-        // do admittance control on the object, calculate the virtual pose as attractor for the impedance controller
-        Eigen::VectorXf objPoseError(6);
-        objPoseError.head(3) = virtualPose.block<3, 1>(0, 3) - objTargetPose.block<3, 1>(0, 3);
-        Eigen::Matrix3f objDiffMat = virtualPose.block<3, 3>(0, 0) * objTargetPose.block<3, 3>(0, 0).transpose();
-        objPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
-
-
-        Eigen::VectorXf objAcc;
-        Eigen::VectorXf objVel;
-        objAcc.setZero(6);
-        objVel.setZero(6);
-        for (size_t i = 0; i < 6; i++)
-        {
-            // objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * (virtualVel(i) - boxTwist(i));
-            objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * virtualVel(i);
-        }
-        objVel = virtualVel + 0.5 * deltaT * (objAcc + virtualAcc);
-        Eigen::VectorXf deltaObjPose = 0.5 * deltaT * (objVel + virtualVel);
-        virtualAcc = objAcc;
-        virtualVel = objVel;
-        virtualPose.block<3, 1>(0, 3) += deltaObjPose.head(3);
-        virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) * virtualPose.block<3, 3>(0, 0);
-
-        // --------------------------------------------- convert to tcp pose ---------------------------------------------
-        // [R_v, P_v // 0, 1] * [R_l P_l// 0, 1] = [R_v*R_l R_v * P_l + P_v]
-        Eigen::Matrix4f tcpTargetPoseLeft = virtualPose * leftPoseInObj;
-        Eigen::Matrix4f tcpTargetPoseRight = virtualPose * rightPoseInObj;
-        //        tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
-        //        tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
-
-
-        // --------------------------------------------- Impedance control ---------------------------------------------
-        Eigen::VectorXf poseError(12);
-        Eigen::Matrix3f diffMat = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(0, 0) = tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
-        poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        diffMat = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
-        poseError.block<3, 1>(6, 0) = tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
-        poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        Eigen::VectorXf forceImpedance(12);
-        for (size_t i = 0; i < 12; i++)
-        {
-            forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
-        }
-
-        // --------------------------------------------- Nullspace control ---------------------------------------------
-        Eigen::VectorXf leftNullspaceTorque = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
-        Eigen::VectorXf rightNullspaceTorque = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
-
-        // --------------------------------------------- Set Torque Control Command ---------------------------------------------
-        //        float lambda = 1;
-        Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
-        Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
-        Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceImpedance.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
-        Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceImpedance.tail(6) + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
-        // torque limit
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            float desiredTorque = leftJointDesiredTorques(i);
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-            desiredTorque = (desiredTorque >  cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
-            rt2DebugBuffer.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
-            leftTargets.at(i)->torque = desiredTorque;
-        }
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            float desiredTorque = rightJointDesiredTorques(i);
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-            desiredTorque = (desiredTorque >   cfg->torqueLimit) ?  cfg->torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < - cfg->torqueLimit) ? - cfg->torqueLimit : desiredTorque;
-            rt2DebugBuffer.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
-            rightTargets.at(i)->torque = desiredTorque;
-        }
-
-        // --------------------------------------------- debug output ---------------------------------------------
-        // impedance control, target TS force
-        rt2DebugBuffer.getWriteBuffer().forceImpedance = forceImpedance;
-        rt2DebugBuffer.getWriteBuffer().poseError = poseError;
-        rt2DebugBuffer.getWriteBuffer().forceTorqueMeasurementInRoot = forceTorqueMeasurementInRoot;
-
-        // object current pose and current twist
-        rt2DebugBuffer.getWriteBuffer().objPoseVec = eigenPose2EigenVec(objCurrentPose);
-        rt2DebugBuffer.getWriteBuffer().objCurrentTwist = objCurrentTwist;
-
-        // object force and torque
-        rt2DebugBuffer.getWriteBuffer().objForceTorque = objFTValue;
-
-        // virtual pose, vel and acc
-        rt2DebugBuffer.getWriteBuffer().virtualPoseVec = eigenPose2EigenVec(virtualPose);
-        rt2DebugBuffer.getWriteBuffer().virtualVel = virtualVel;
-        rt2DebugBuffer.getWriteBuffer().virtualAcc = virtualAcc;
-
-        // integrated pose
-        rt2DebugBuffer.getWriteBuffer().integratedPoseObjVec = eigenPose2EigenVec(integratedPoseObj);
-        rt2DebugBuffer.getWriteBuffer().integratedPoseLeftVec = eigenPose2EigenVec(integratedPoseLeft);
-        rt2DebugBuffer.getWriteBuffer().integratedPoseRightVec = eigenPose2EigenVec(integratedPoseRight);
-
-
-        // hand poses
-        rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootLeft   = eigenPose2EigenVec(tcpTargetPoseLeft);
-        rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootRight  = eigenPose2EigenVec(tcpTargetPoseRight);
-        rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootLeft  = eigenPose2EigenVec(currentLeftPose);
-        rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootRight = eigenPose2EigenVec(currentRightPose);
-
-        // dmp targets
-        rt2DebugBuffer.getWriteBuffer().objTargetPoseVec = eigenPose2EigenVec(objTargetPose);
-        rt2DebugBuffer.getWriteBuffer().leftPoseVecInObj = eigenPose2EigenVec(leftPoseInObj);
-        rt2DebugBuffer.getWriteBuffer().rightPoseVecInObj = eigenPose2EigenVec(rightPoseInObj);
-        rt2DebugBuffer.getWriteBuffer().objTargetTwist = objTargetTwist;
-
-        // parameters
-        rt2DebugBuffer.getWriteBuffer().KpImpedance = KpImpedance;
-        rt2DebugBuffer.getWriteBuffer().KdImpedance = KdImpedance;
-        rt2DebugBuffer.getWriteBuffer().KpAdmittance = KpAdmittance;
-        rt2DebugBuffer.getWriteBuffer().KdAdmittance = KdAdmittance;
-        rt2DebugBuffer.getWriteBuffer().KmAdmittance = KmAdmittance;
-        rt2DebugBuffer.getWriteBuffer().KmPID = KmPID;
-
-        rt2DebugBuffer.commitWrite();
-
-    }
-
-    void NJointBimanualObjLevelMultiMPController::getObjStatus(Eigen::Matrix4f& pose, Eigen::VectorXf& twist)
-    {
-        // this is a temporary function to get status of the object,
-        // in fact, this should be set via the interface function.
-        Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
-        leftPose.block<3, 1>(0, 3) *= 0.001;
-        pose = leftPose * objTransMatrixInAnchor;
-        twist.setZero(6);
-    }
-
-    // TODO
-    //    void NJointBimanualObjLevelMultiMPController::setObjStatus()
-    //    {
-    //        // ice interface function, create a buffer for this
-    //    }
-
-
-    void NJointBimanualObjLevelMultiMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        objectDMP->learnDMPFromFiles(fileNames);
-    }
-
-    void NJointBimanualObjLevelMultiMPController::learnMultiDMPFromFiles(const Ice::StringSeq& objFileNames, const Ice::StringSeq& leftFileNames, const Ice::StringSeq& rightFileNames, const Ice::Current&)
-    {
-        objectDMP->learnDMPFromFiles(objFileNames);
-        leftTCPInObjDMP->learnDMPFromFiles(leftFileNames);
-        rightTCPInObjDMP->learnDMPFromFiles(rightFileNames);
-    }
-
-    void NJointBimanualObjLevelMultiMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard(controllerMutex);
-        objectDMP->setGoalPoseVec(goals);
-    }
-
-    void NJointBimanualObjLevelMultiMPController::setMultiMPGoals(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, const Ice::Current& ice)
-    {
-        LockGuardType guard(controllerMutex);
-        objectDMP->setGoalPoseVec(goalObj);
-        leftTCPInObjDMP->setGoalPoseVec(goalLeft);
-        rightTCPInObjDMP->setGoalPoseVec(goalRight);
-    }
-
-    void NJointBimanualObjLevelMultiMPController::integrateVel2Pose(const double deltaT, Eigen::VectorXf& vel, Eigen::Matrix4f& pose)
-    {
-        Eigen::Matrix3f deltaRot = VirtualRobot::MathTools::rpy2eigen3f(vel.tail(3) * static_cast<float>(deltaT));
-        pose.block<3, 3>(0, 0) = deltaRot * pose.block<3, 3>(0, 0);
-        pose.block<3, 1>(0, 3) += vel.head(3) * static_cast<float>(deltaT);
-    }
-
-    std::vector<double> NJointBimanualObjLevelMultiMPController::eigenPose2Vec(const Eigen::Matrix4f& pose)
-    {
-        VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
-        std::vector<double> poseVec {pose(0, 3), pose(1, 3), pose(2, 3), ori.w, ori.x, ori.y, ori.z};
-        return poseVec;
-    }
-
-    Eigen::VectorXf NJointBimanualObjLevelMultiMPController::eigenPose2EigenVec(const Eigen::Matrix4f& pose)
-    {
-        VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
-        Eigen::VectorXf poseVec;
-        poseVec.setZero(7);
-        poseVec(0) = pose(0, 3);
-        poseVec(1) = pose(1, 3);
-        poseVec(2) = pose(2, 3);
-        poseVec(3) = ori.w;
-        poseVec(4) = ori.x;
-        poseVec(5) = ori.y;
-        poseVec(6) = ori.z;
-        return poseVec;
-    }
-
-    void NJointBimanualObjLevelMultiMPController::runDMP(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, Ice::Double timeDuration, const Ice::Current&)
-    {
-        while (!rt2InterfaceBuffer.updateReadBuffer())
-        {
-            usleep(1000);
-        }
-
-        Eigen::Matrix4f leftInitPose  = rt2InterfaceBuffer.getUpToDateReadBuffer().currentLeftPoseInObjFrame;
-        Eigen::Matrix4f rightInitPose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentRightPoseInObjFrame;
-        Eigen::Matrix4f objInitPose   = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjPose;
-        std::vector<double> objInitPoseVec   = eigenPose2Vec(objInitPose);
-        std::vector<double> leftInitPoseVec  = eigenPose2Vec(leftInitPose);
-        std::vector<double> rightInitPoseVec = eigenPose2Vec(rightInitPose);
-        ARMARX_INFO << "get init poses: \n" << VAROUT(objInitPoseVec) << "\n" << VAROUT(leftInitPoseVec) << "\n" << VAROUT(rightInitPoseVec);
-
-        // set the integrated left/right pose when start to run dmp
-        integratedPoseObj = objInitPose;
-        integratedPoseLeft = leftInitPose;
-        integratedPoseRight = rightInitPose;
-
-        objectDMP->prepareExecution(objInitPoseVec, goalObj);
-        objectDMP->canVal = timeDuration;
-        objectDMP->config.motionTimeDuration = timeDuration;
-
-        leftTCPInObjDMP->prepareExecution(leftInitPose, getLocalPose(goalObj, goalLeft));
-        leftTCPInObjDMP->canVal = timeDuration;
-        leftTCPInObjDMP->config.motionTimeDuration = timeDuration;
-
-        rightTCPInObjDMP->prepareExecution(rightInitPose, getLocalPose(goalObj, goalRight));
-        rightTCPInObjDMP->canVal = timeDuration;
-        rightTCPInObjDMP->config.motionTimeDuration = timeDuration;
-        ARMARX_INFO << "DMP prepare execution is done";
-
-        finished = false;
-        dmpStarted = true;
-    }
-
-
-    Eigen::Matrix4f NJointBimanualObjLevelMultiMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
-    {
-        Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
-
-        localPose.block<3, 3>(0, 0) = newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
-        localPose.block<3, 1>(0, 3) = newCoordinate.block<3, 3>(0, 0).inverse() * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
-
-
-        return localPose;
-    }
-
-    Eigen::Matrix4f NJointBimanualObjLevelMultiMPController::getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
-    {
-        Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
-        newCoordinate(0, 3) = newCoordinateVec.at(0);
-        newCoordinate(1, 3) = newCoordinateVec.at(1);
-        newCoordinate(2, 3) = newCoordinateVec.at(2);
-
-        Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
-        globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
-        globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
-        globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
-
-        return getLocalPose(newCoordinate, globalTargetPose);
-
-    }
-
-    void NJointBimanualObjLevelMultiMPController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
-    {
-        ARMARX_ERROR << "implementation not complete!!!";
-        while (!rt2InterfaceBuffer.updateReadBuffer())
-        {
-            usleep(1000);
-        }
-        ARMARX_IMPORTANT << "obj level control: setup dmp ...";
-        objectDMP->prepareExecution(starts, goals);
-        objectDMP->canVal = timeDuration;
-        objectDMP->config.motionTimeDuration = timeDuration;
-
-        finished = false;
-        dmpStarted = true;
-
-        ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
-    }
-
-    void NJointBimanualObjLevelMultiMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-        //        LockGuardType guard(controllerMutex);
-        ARMARX_INFO << "setting via points ";
-        objectDMP->setViaPose(u, viapoint);
-    }
-
-    void NJointBimanualObjLevelMultiMPController::removeAllViaPoints(const Ice::Current&)
-    {
-        objectDMP->removeAllViaPoints();
-    }
-
-    void NJointBimanualObjLevelMultiMPController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 12);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
-        interface2rtBuffer.commitWrite();
-
-    }
-
-    void NJointBimanualObjLevelMultiMPController::setAmplitude(Ice::Double amp, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        if (objectDMP->config.DMPStyle == "Periodic")
-        {
-            objectDMP->setAmplitude(amp);
-        }
-
-        if (leftTCPInObjDMP->config.DMPStyle == "Periodic")
-        {
-            leftTCPInObjDMP->setAmplitude(amp);
-        }
-
-        if (rightTCPInObjDMP->config.DMPStyle == "Periodic")
-        {
-            rightTCPInObjDMP->setAmplitude(amp);
-        }
-    }
-
-    void NJointBimanualObjLevelMultiMPController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 12);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-    void NJointBimanualObjLevelMultiMPController::setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-    void NJointBimanualObjLevelMultiMPController::setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-    void NJointBimanualObjLevelMultiMPController::setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-    std::vector<float> NJointBimanualObjLevelMultiMPController::getCurrentObjVel(const Ice::Current&)
-    {
-        Eigen::Vector3f tvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
-        std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
-        return tvelvec;
-    }
-
-    std::vector<float> NJointBimanualObjLevelMultiMPController::getCurrentObjForce(const Ice::Current&)
-    {
-        Eigen::Vector3f fvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
-        std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
-        return fvelvec;
-    }
-
-    void NJointBimanualObjLevelMultiMPController::publishVec(const Eigen::VectorXf& bufferVec, const std::string name, StringVariantBaseMap& datafields)
-    {
-        for (int i = 0; i < bufferVec.rows(); ++i)
-        {
-            std::string data_name = name + "_" + std::to_string(i);
-            datafields[data_name] = new Variant(bufferVec(i));
-        }
-    }
-
-    void NJointBimanualObjLevelMultiMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-
-        StringVariantBaseMap datafields;
-        auto values = rt2DebugBuffer.getUpToDateReadBuffer().desired_torques;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance, "forceImpedance", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forcePID, "forcePID", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().poseError, "poseError", datafields);
-
-        // ------------------ force torque measurement of hands ------------------
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot, "forceTorqueMeasurementInRoot", datafields);
-
-        // ------------------ object force torques ------------------
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque, "objForceTorque", datafields);
-
-        // ------------------ virtual pose, vel and acc ------------------
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec, "virtualPoseVec", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().virtualVel, "virtualVel", datafields);
-
-        // ------------------ object pose vec, vel ------------------
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec, "objPoseVec", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist, "objCurrentTwist", datafields);
-
-        // ------------------ hand poses ------------------
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft, "targetHandPoseInRootLeft", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight, "targetHandPoseInRootRight", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft, "currentHandPoseInRootLeft", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight, "currentHandPoseInRootRight", datafields);
-
-        // ------------------ dmp targets ------------------
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec, "objTargetPoseVec", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj, "leftPoseVecInObj", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj, "rightPoseVecInObj", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist, "objTargetTwist", datafields);
-
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseObjVec, "integratedPoseObjVec", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseLeftVec, "integratedPoseLeftVec", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseRightVec, "integratedPoseRightVec", datafields);
-
-        // ------------------ controller parameters ------------------
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance, "KpImpedance", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance, "KdImpedance", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance, "KpAdmittance", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance, "KdAdmittance", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance, "KmAdmittance", datafields);
-        publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmPID, "KmPID", datafields);
-
-        //        Eigen::VectorXf forceImpedance = rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance;
-        //        for (int i = 0; i < forceImpedance.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "forceImpedance_" + ss.str();
-        //            datafields[data_name] = new Variant(forceImpedance(i));
-        //        }
-
-        //        Eigen::VectorXf forcePID = rt2DebugBuffer.getUpToDateReadBuffer().forcePID;
-        //        for (int i = 0; i < forcePID.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "forcePID_" + ss.str();
-        //            datafields[data_name] = new Variant(forcePID(i));
-        //        }
-
-
-        //        Eigen::VectorXf poseError = rt2DebugBuffer.getUpToDateReadBuffer().poseError;
-        //        for (int i = 0; i < poseError.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "poseError_" + ss.str();
-        //            datafields[data_name] = new Variant(poseError(i));
-        //        }
-
-        //        // ------------------ force torque measurement of hands ------------------
-        //        Eigen::VectorXf forceTorqueMeasurementInRoot = rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot;
-        //        for (int i = 0; i < forceTorqueMeasurementInRoot.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "forceTorqueMeasurementInRoot_" + ss.str();
-        //            datafields[data_name] = new Variant(forceTorqueMeasurementInRoot(i));
-        //        }
-
-        // ------------------ object force torques ------------------
-        //        Eigen::VectorXf objForceTorque = rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque;
-        //        for (int i = 0; i < objForceTorque.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "objForceTorque_" + ss.str();
-        //            datafields[data_name] = new Variant(objForceTorque(i));
-        //        }
-
-        // ------------------ virtual pose, vel and acc ------------------
-        //        Eigen::VectorXf virtualPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec;
-        //        for (int i = 0; i < virtualPoseVec.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "virtualPoseVec_" + ss.str();
-        //            datafields[data_name] = new Variant(virtualPoseVec(i));
-        //        }
-
-        //        Eigen::VectorXf virtualVel = rt2DebugBuffer.getUpToDateReadBuffer().virtualVel;
-        //        for (int i = 0; i < virtualVel.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "virtualVel_" + ss.str();
-        //            datafields[data_name] = new Variant(virtualVel(i));
-        //        }
-
-        // ------------------ object pose vec, vel ------------------
-        //        Eigen::VectorXf objPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec;
-        //        for (int i = 0; i < objPoseVec.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "objPoseVec_" + ss.str();
-        //            datafields[data_name] = new Variant(objPoseVec(i));
-        //        }
-
-        //        Eigen::VectorXf objCurrentTwist = rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist;
-        //        for (int i = 0; i < objCurrentTwist.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "objCurrentTwist_" + ss.str();
-        //            datafields[data_name] = new Variant(objCurrentTwist(i));
-        //        }
-
-        // ------------------ hand poses ------------------
-        //        Eigen::VectorXf targetHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft;
-        //        for (int i = 0; i < targetHandPoseInRootLeft.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "targetHandPoseInRootLeft_" + ss.str();
-        //            datafields[data_name] = new Variant(targetHandPoseInRootLeft(i));
-        //        }
-        //        Eigen::VectorXf targetHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight;
-        //        for (int i = 0; i < targetHandPoseInRootRight.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "targetHandPoseInRootRight_" + ss.str();
-        //            datafields[data_name] = new Variant(targetHandPoseInRootRight(i));
-        //        }
-        //        Eigen::VectorXf currentHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft;
-        //        for (int i = 0; i < currentHandPoseInRootLeft.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "currentHandPoseInRootLeft_" + ss.str();
-        //            datafields[data_name] = new Variant(currentHandPoseInRootLeft(i));
-        //        }
-        //        Eigen::VectorXf currentHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight;
-        //        for (int i = 0; i < currentHandPoseInRootRight.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "currentHandPoseInRootRight_" + ss.str();
-        //            datafields[data_name] = new Variant(currentHandPoseInRootRight(i));
-        //        }
-        // ------------------ dmp targets ------------------
-        //        Eigen::VectorXf objTargetPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec;
-        //        for (int i = 0; i < objTargetPoseVec.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "objTargetPoseVec_" + ss.str();
-        //            datafields[data_name] = new Variant(objTargetPoseVec(i));
-        //        }
-        //        Eigen::VectorXf leftPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj;
-        //        for (int i = 0; i < leftPoseVecInObj.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "leftPoseVecInObj_" + ss.str();
-        //            datafields[data_name] = new Variant(leftPoseVecInObj(i));
-        //        }
-        //        Eigen::VectorXf rightPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj;
-        //        for (int i = 0; i < rightPoseVecInObj.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "rightPoseVecInObj_" + ss.str();
-        //            datafields[data_name] = new Variant(rightPoseVecInObj(i));
-        //        }
-        //        Eigen::VectorXf objTargetTwist = rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist;
-        //        for (int i = 0; i < objTargetTwist.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "objTargetTwist_" + ss.str();
-        //            datafields[data_name] = new Variant(objTargetTwist(i));
-        //        }
-
-        // parameters
-        //        Eigen::VectorXf KpImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance;
-        //        for (int i = 0; i < KpImpedance.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "KpImpedance_" + ss.str();
-        //            datafields[data_name] = new Variant(KpImpedance(i));
-        //        }
-        //        Eigen::VectorXf KdImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance;
-        //        for (int i = 0; i < KdImpedance.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "KdImpedance_" + ss.str();
-        //            datafields[data_name] = new Variant(KdImpedance(i));
-        //        }
-        //        Eigen::VectorXf KpAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance;
-        //        for (int i = 0; i < KpAdmittance.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "KpAdmittance_" + ss.str();
-        //            datafields[data_name] = new Variant(KpAdmittance(i));
-        //        }
-        //        Eigen::VectorXf KdAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance;
-        //        for (int i = 0; i < KdAdmittance.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "KdAdmittance_" + ss.str();
-        //            datafields[data_name] = new Variant(KdAdmittance(i));
-        //        }
-        //        Eigen::VectorXf KmAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance;
-        //        for (int i = 0; i < KmAdmittance.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "KmAdmittance_" + ss.str();
-        //            datafields[data_name] = new Variant(KmAdmittance(i));
-        //        }
-        //        Eigen::VectorXf KmPID = rt2DebugBuffer.getUpToDateReadBuffer().KmPID;
-        //        for (int i = 0; i < KmPID.rows(); ++i)
-        //        {
-        //            std::stringstream ss;
-        //            ss << i;
-        //            std::string data_name = "KmPID_" + ss.str();
-        //            datafields[data_name] = new Variant(KmPID(i));
-        //        }
-
-        debugObs->setDebugChannel("BimanualForceController", datafields);
-    }
-
-    void NJointBimanualObjLevelMultiMPController::onInitNJointController()
-    {
-        ARMARX_INFO << "====== init bimanual multi mp controller ======";
-        runTask("NJointBimanualObjLevelMultiMPController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-    }
-
-    void NJointBimanualObjLevelMultiMPController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "====== stop bimanual multi mp controller ======";
-    }
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
deleted file mode 100644
index 1fbd032b02b4ce2644e4fc57876e548c531c69aa..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
+++ /dev/null
@@ -1,297 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
-    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelMultiMPController);
-    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelMultiMPControlData);
-
-    class NJointBimanualObjLevelMultiMPControlData
-    {
-    public:
-        // control target from Movement Primitives
-        Eigen::Matrix4f objTargetPose;
-        Eigen::VectorXf objTargetTwist;
-
-        Eigen::Matrix4f leftTargetPoseInObj;
-        Eigen::VectorXf leftTargetTwistInObj;
-
-        Eigen::Matrix4f rightTargetPoseInObj;
-        Eigen::VectorXf rightTargetTwistInObj;
-    };
-
-
-    class NJointBimanualObjLevelMultiMPController :
-        public NJointControllerWithTripleBuffer<NJointBimanualObjLevelMultiMPControlData>,
-        public NJointBimanualObjLevelMultiMPControllerInterface
-    {
-    public:
-        //        using ConfigPtrT = BimanualForceControllerConfigPtr;
-        NJointBimanualObjLevelMultiMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointCCDMPControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
-        void learnMultiDMPFromFiles(const Ice::StringSeq& objFileNames, const Ice::StringSeq& leftFileNames, const Ice::StringSeq& rightFileNames, const Ice::Current&);
-
-        bool isFinished(const Ice::Current&)
-        {
-            return finished;
-        }
-        Eigen::Matrix3f skew(Eigen::Vector3f vec)
-        {
-            Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
-            mat(1, 2) = -vec(0);
-            mat(0, 2) = vec(1);
-            mat(0, 1) = -vec(2);
-            mat(2, 1) = vec(0);
-            mat(2, 0) = -vec(1);
-            mat(1, 0) = vec(2);
-            return mat;
-        }
-
-        //        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
-        void runDMP(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, Ice::Double timeDuration, const Ice::Current&);
-        void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void setMultiMPGoals(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, const Ice::Current& ice);
-
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
-        void removeAllViaPoints(const Ice::Current&);
-
-        double getVirtualTime(const Ice::Current&)
-        {
-            return virtualtimer;
-        }
-
-        void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
-        void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
-        void setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
-        void setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
-        void setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
-
-        std::vector<float> getCurrentObjVel(const Ice::Current&);
-        std::vector<float> getCurrentObjForce(const Ice::Current&);
-
-        void getObjStatus(Eigen::Matrix4f& pose, Eigen::VectorXf& twist);
-        std::vector<double> eigenPose2Vec(const Eigen::Matrix4f& pose);
-        Eigen::VectorXf eigenPose2EigenVec(const Eigen::Matrix4f& pose);
-        Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
-        Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec);
-        void integrateVel2Pose(const double deltaT, Eigen::VectorXf& vel, Eigen::Matrix4f& pose);
-        void publishVec(const Eigen::VectorXf& bufferVec, const std::string name, StringVariantBaseMap& datafields);
-        void setAmplitude(Ice::Double amp, const Ice::Current&);
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitNJointController();
-        void onDisconnectNJointController();
-        void controllerRun();
-
-    private:
-        Eigen::VectorXf targetWrench;
-        struct RT2DebugData
-        {
-            StringFloatDictionary desired_torques;
-
-            // dmp targets
-            Eigen::VectorXf objTargetPoseVec;
-            Eigen::VectorXf leftPoseVecInObj;
-            Eigen::VectorXf rightPoseVecInObj;
-            Eigen::VectorXf objTargetTwist;
-
-            // hand poses
-            Eigen::VectorXf targetHandPoseInRootLeft;
-            Eigen::VectorXf targetHandPoseInRootRight;
-            Eigen::VectorXf currentHandPoseInRootLeft;
-            Eigen::VectorXf currentHandPoseInRootRight;
-
-            // object pose, vel and force torque
-            Eigen::VectorXf objForceTorque;
-            Eigen::VectorXf objPoseVec;
-            Eigen::VectorXf objCurrentTwist;
-
-            // virtual pose, vel, acc
-            Eigen::VectorXf virtualPoseVec;
-            Eigen::VectorXf virtualVel;
-            Eigen::VectorXf virtualAcc;
-
-            // integrated pose
-            Eigen::VectorXf integratedPoseObjVec;
-            Eigen::VectorXf integratedPoseLeftVec;
-            Eigen::VectorXf integratedPoseRightVec;
-
-            Eigen::VectorXf poseError;
-
-            // force
-            Eigen::VectorXf forceImpedance;
-            Eigen::VectorXf forcePID;
-            Eigen::VectorXf forcePIDControlValue;
-            Eigen::VectorXf forceTorqueMeasurementInRoot;
-
-            // parameters
-            Eigen::VectorXf KpImpedance;
-            Eigen::VectorXf KdImpedance;
-            Eigen::VectorXf KpAdmittance;
-            Eigen::VectorXf KdAdmittance;
-            Eigen::VectorXf KmAdmittance;
-            Eigen::VectorXf KmPID;
-        };
-        TripleBuffer<RT2DebugData> rt2DebugBuffer;
-
-        struct RT2ControlData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPoseObj;
-            Eigen::VectorXf currentTwistObj;
-
-            Eigen::Matrix4f currentPoseLeftInObj;
-            Eigen::VectorXf currentTwistLeftInObj;
-
-            Eigen::Matrix4f currentPoseRightInObj;
-            Eigen::VectorXf currentTwistRightInObj;
-        };
-        TripleBuffer<RT2ControlData> rt2ControlBuffer;
-
-        struct RT2InterfaceData
-        {
-            Eigen::Matrix4f currentLeftPoseInObjFrame;
-            Eigen::Matrix4f currentRightPoseInObjFrame;
-            Eigen::Matrix4f currentObjPose;
-            Eigen::Vector3f currentObjVel;
-            Eigen::Vector3f currentObjForce;
-        };
-        TripleBuffer<RT2InterfaceData> rt2InterfaceBuffer;
-
-        struct Inferface2rtData
-        {
-            Eigen::VectorXf KpImpedance;
-            Eigen::VectorXf KdImpedance;
-            Eigen::VectorXf KmAdmittance;
-            Eigen::VectorXf KpAdmittance;
-            Eigen::VectorXf KdAdmittance;
-        };
-        TripleBuffer<Inferface2rtData> interface2rtBuffer;
-
-
-
-        std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
-        std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
-        const SensorValueForceTorque* rightForceTorque;
-        const SensorValueForceTorque* leftForceTorque;
-
-        NJointBimanualObjLevelMultiMPControllerConfigPtr cfg;
-        VirtualRobot::DifferentialIKPtr leftIK;
-        VirtualRobot::DifferentialIKPtr rightIK;
-
-        TaskSpaceDMPControllerPtr objectDMP;
-        TaskSpaceDMPControllerPtr leftTCPInObjDMP;
-        TaskSpaceDMPControllerPtr rightTCPInObjDMP;
-
-        Eigen::Matrix4f objInitialPose;   // in root frame
-        Eigen::Matrix4f leftInitialPose;  // in obj frame
-        Eigen::Matrix4f rightInitialPose; // in obj frame
-
-        // add integrated pose by accumulating dmp target velocity to initial pose
-        Eigen::Matrix4f integratedPoseObj;
-        Eigen::Matrix4f integratedPoseLeft;
-        Eigen::Matrix4f integratedPoseRight;
-
-        Eigen::Matrix4f objTransMatrixInAnchor;
-
-        double virtualtimer;
-
-        mutable MutexType controllerMutex;
-        mutable MutexType interfaceDataMutex;
-        Eigen::VectorXf leftDesiredJointValues;
-        Eigen::VectorXf rightDesiredJointValues;
-
-        Eigen::VectorXf KpImpedance;
-        Eigen::VectorXf KdImpedance;
-        Eigen::VectorXf KpAdmittance;
-        Eigen::VectorXf KdAdmittance;
-        Eigen::VectorXf KmAdmittance;
-        Eigen::VectorXf KmPID;
-
-        Eigen::VectorXf virtualAcc;
-        Eigen::VectorXf virtualVel;
-        Eigen::Matrix4f virtualPose;
-
-        Eigen::Matrix4f sensorFrame2TcpFrameLeft;
-        Eigen::Matrix4f sensorFrame2TcpFrameRight;
-
-        //static compensation
-        float massLeft;
-        Eigen::Vector3f CoMVecLeft;
-        Eigen::Vector3f forceOffsetLeft;
-        Eigen::Vector3f torqueOffsetLeft;
-
-        float massRight;
-        Eigen::Vector3f CoMVecRight;
-        Eigen::Vector3f forceOffsetRight;
-        Eigen::Vector3f torqueOffsetRight;
-
-        //        float knull;
-        //        float dnull;
-
-        std::vector<std::string> leftJointNames;
-        std::vector<std::string> rightJointNames;
-
-        //        float torqueLimit;
-        VirtualRobot::RobotNodeSetPtr leftRNS;
-        VirtualRobot::RobotNodeSetPtr rightRNS;
-        VirtualRobot::RobotNodePtr tcpLeft;
-        VirtualRobot::RobotNodePtr tcpRight;
-
-        std::vector<PIDControllerPtr> forcePIDControllers;
-
-        // filter parameters
-        float filterCoeff;
-        Eigen::VectorXf filteredOldValue;
-        bool finished;
-        bool dmpStarted;
-        double ftcalibrationTimer;
-        Eigen::VectorXf ftOffset;
-
-        Eigen::Matrix3f fixedLeftRightRotOffset;
-        Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
-
-    protected:
-        void rtPreActivateController();
-        bool firstLoop;
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
deleted file mode 100644
index bceb67e36837a6444a2d64249f7088389d019b26..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
+++ /dev/null
@@ -1,730 +0,0 @@
-#include "NJointBimanualObjLevelVelController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <VirtualRobot/MathTools.h>
-#include <VirtualRobot/VirtualRobot.h>
-namespace armarx
-{
-    NJointControllerRegistration<NJointBimanualObjLevelVelController> registrationControllerNJointBimanualObjLevelVelController("NJointBimanualObjLevelVelController");
-
-    NJointBimanualObjLevelVelController::NJointBimanualObjLevelVelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "Preparing ... bimanual ";
-        ARMARX_INFO << "I am here";
-
-        useSynchronizedRtRobot();
-        cfg = NJointBimanualObjLevelVelControllerConfigPtr::dynamicCast(config);
-        //        ARMARX_CHECK_EXPRESSION(prov);
-        //        RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
-        //        ARMARX_CHECK_EXPRESSION(robotUnit);
-        leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-
-        for (size_t i = 0; i < leftRNS->getSize(); ++i)
-        {
-            std::string jointName = leftRNS->getNode(i)->getName();
-            leftJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-
-            leftVelocitySensors.push_back(velocitySensor);
-            leftPositionSensors.push_back(positionSensor);
-
-        };
-
-        rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
-        for (size_t i = 0; i < rightRNS->getSize(); ++i)
-        {
-            std::string jointName = rightRNS->getNode(i)->getName();
-            rightJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-            rightVelocitySensors.push_back(velocitySensor);
-            rightPositionSensors.push_back(positionSensor);
-
-        };
-
-        leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-        leftTCPController.reset(new CartesianVelocityController(leftRNS, leftRNS->getTCP()));
-        rightTCPController.reset(new CartesianVelocityController(rightRNS, rightRNS->getTCP()));
-
-        double phaseL = 10;
-        double phaseK = 10;
-        double phaseDist0 = 50;
-        double phaseDist1 = 10;
-        double phaseKpPos = 1;
-        double phaseKpOri = 0.1;
-        double posToOriRatio = 10;
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
-        taskSpaceDMPConfig.DMPAmplitude = 1.0;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.goDist = phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Kori = phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = phaseK;
-
-
-        objectDMP.reset(new TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
-        ARMARX_IMPORTANT << "dmp finished";
-
-        tcpLeft = leftRNS->getTCP();
-        tcpRight = rightRNS->getTCP();
-
-        //initialize control parameters
-        KpImpedance.setZero(cfg->KpImpedance.size());
-        ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
-
-        for (int i = 0; i < KpImpedance.size(); ++i)
-        {
-            KpImpedance(i) = cfg->KpImpedance.at(i);
-        }
-
-        KdImpedance.setZero(cfg->KdImpedance.size());
-        ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 6);
-
-        for (int i = 0; i < KdImpedance.size(); ++i)
-        {
-            KdImpedance(i) = cfg->KdImpedance.at(i);
-        }
-
-        Inferface2rtData initInt2rtData;
-        initInt2rtData.KpImpedance = KpImpedance;
-        initInt2rtData.KdImpedance = KdImpedance;
-        interface2rtBuffer.reinitAllBuffers(initInt2rtData);
-
-        leftDesiredJointValues.resize(leftTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
-        }
-
-        rightDesiredJointValues.resize(rightTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
-        }
-
-        virtualPose = Eigen::Matrix4f::Identity();
-        ARMARX_INFO << "got controller params";
-
-        rt2ControlData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentPose = leftRNS->getTCP()->getPoseInRootFrame();
-        initSensorData.currentTwist.setZero();
-        rt2ControlBuffer.reinitAllBuffers(initSensorData);
-
-
-        ControlInterfaceData initInterfaceData;
-        initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
-        initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
-        initInterfaceData.currentObjPose = leftRNS->getTCP()->getPoseInRootFrame();
-        initInterfaceData.currentObjVel.setZero();
-        controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
-
-
-        leftInitialPose = tcpLeft->getPoseInRootFrame();
-        rightInitialPose = tcpRight->getPoseInRootFrame();
-        leftInitialPose.block<3, 1>(0, 3) =  leftInitialPose.block<3, 1>(0, 3);
-        rightInitialPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3);
-
-        // TODO the following is only predefined for balance ball
-        fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
-
-        Eigen::Matrix4f rightLeveledRotation = VirtualRobot::MathTools::quat2eigen4f(0.5, -0.5, -0.5, -0.5);
-        Eigen::Matrix4f leftLeveledRotation = VirtualRobot::MathTools::quat2eigen4f(0.5, 0.5, 0.5, -0.5);
-        fixedLeftRightRotOffset =  leftLeveledRotation.block<3, 3>(0, 0).transpose() * rightLeveledRotation.block<3, 3>(0, 0);
-
-
-        boxInitialPose = leftInitialPose;
-        boxInitialPose(0, 3) = (leftInitialPose(0, 3) + rightInitialPose(0, 3)) / 2;
-        boxInitialPose(1, 3) = (leftInitialPose(1, 3) + rightInitialPose(1, 3)) / 2;
-        boxInitialPose(2, 3) = (leftInitialPose(2, 3) + rightInitialPose(2, 3)) / 2;
-
-        NJointBimanualObjLevelVelControlData initData;
-        initData.boxPose = boxInitialPose;
-        reinitTripleBuffer(initData);
-
-        dmpGoal = boxInitialPose;
-
-        firstLoop = true;
-        ARMARX_INFO << "left initial pose: \n" << leftInitialPose  << "\n right initial pose: \n" << rightInitialPose;
-        dmpStarted = false;
-        objCom2TCPLeftInObjFrame.setZero();
-        objCom2TCPRightInObjFrame.setZero();
-
-    }
-
-    void NJointBimanualObjLevelVelController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
-    {
-        objectDMP->setWeights(weights);
-    }
-
-    DoubleSeqSeq NJointBimanualObjLevelVelController::getMPWeights(const Ice::Current&)
-    {
-        DMP::DVec2d res = objectDMP->getWeights();
-        DoubleSeqSeq resvec;
-        for (size_t i = 0; i < res.size(); ++i)
-        {
-            std::vector<double> cvec;
-            for (size_t j = 0; j < res[i].size(); ++j)
-            {
-                cvec.push_back(res[i][j]);
-            }
-            resvec.push_back(cvec);
-        }
-
-        return resvec;
-    }
-
-    void NJointBimanualObjLevelVelController::setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&)
-    {
-        objectDMP->setRotWeights(weights);
-    }
-
-    DoubleSeqSeq NJointBimanualObjLevelVelController::getMPRotWeights(const Ice::Current&)
-    {
-        DMP::DVec2d res = objectDMP->getRotWeights();
-        DoubleSeqSeq resvec;
-        for (size_t i = 0; i < res.size(); ++i)
-        {
-            std::vector<double> cvec;
-            for (size_t j = 0; j < res[i].size(); ++j)
-            {
-                cvec.push_back(res[i][j]);
-            }
-            resvec.push_back(cvec);
-        }
-
-        return resvec;
-    }
-
-    void NJointBimanualObjLevelVelController::rtPreActivateController()
-    {
-        Eigen::Matrix4f boxInitPose = Eigen::Matrix4f::Identity();
-        Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
-        leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) ;
-        rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) ;
-        boxInitPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
-        boxInitPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
-
-        NJointBimanualObjLevelVelControlData initData;
-        initData.boxPose = boxInitPose;
-        reinitTripleBuffer(initData);
-    }
-
-    std::string NJointBimanualObjLevelVelController::getClassName(const Ice::Current&) const
-    {
-        return "NJointBimanualObjLevelVelController";
-    }
-
-    void NJointBimanualObjLevelVelController::controllerRun()
-    {
-        if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
-        {
-            return;
-        }
-
-        double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
-        Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
-        Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
-
-        if (objectDMP->canVal < 0)
-        {
-            finished = true;
-            dmpStarted = false;
-            LockGuardType guard {controlDataMutex};
-            getWriterControlStruct().boxPose = dmpGoal;
-            writeControlStruct();
-        }
-        else
-        {
-            objectDMP->flow(deltaT, currentPose, currentTwist);
-            //            VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(objectDMP->getTargetPoseMat());
-            LockGuardType guard {controlDataMutex};
-            getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
-            writeControlStruct();
-        }
-
-    }
-
-
-    Eigen::VectorXf NJointBimanualObjLevelVelController::calcIK(VirtualRobot::DifferentialIKPtr ik, const Eigen::MatrixXf& jacobi, const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel)
-    {
-        Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
-
-        Eigen::MatrixXf nullspace = lu_decomp.kernel();
-        Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
-        for (int i = 0; i < nullspace.cols(); i++)
-        {
-            float squaredNorm = nullspace.col(i).squaredNorm();
-            // Prevent division by zero
-            if (squaredNorm > 1.0e-32f)
-            {
-                nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
-            }
-        }
-
-        Eigen::MatrixXf inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
-        Eigen::VectorXf jointVel = inv * cartesianVel;
-        return jointVel;
-    }
-
-    void NJointBimanualObjLevelVelController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
-
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-        if (firstLoop)
-        {
-            Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
-            Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
-
-            leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3);
-            rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3);
-
-            virtualPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
-            virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
-            //            fixedLeftRightRotOffset =  virtualPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
-
-            Eigen::Vector3f objCom2TCPLeftInGlobal = leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-            Eigen::Vector3f objCom2TCPRightInGlobal = rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-
-            objCom2TCPLeftInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
-            objCom2TCPRightInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
-            firstLoop = false;
-        }
-
-        // --------------------------------------------- get control parameters ---------------------------------------
-        KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
-        KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
-
-        // --------------------------------------------- grasp matrix ---------------------------------------------
-
-        Eigen::MatrixXf graspMatrix;
-        graspMatrix.setZero(6, 12);
-        graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
-        graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
-        Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
-        graspMatrix.block<3, 3>(3, 0) = skew(rvec);
-
-        rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
-        graspMatrix.block<3, 3>(3, 6) = skew(rvec);
-
-        float lambda = 1;
-        Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
-
-        // ---------------------------------------------- object pose ----------------------------------------------
-        Eigen::Matrix4f boxCurrentPose = currentLeftPose;
-        boxCurrentPose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
-        Eigen::VectorXf boxCurrentTwist;
-        boxCurrentTwist.setZero(6);
-
-        // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-        // Jacobian matrices
-        Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-        Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        // qpos, qvel
-        Eigen::VectorXf leftqpos;
-        Eigen::VectorXf leftqvel;
-        leftqpos.resize(leftPositionSensors.size());
-        leftqvel.resize(leftVelocitySensors.size());
-        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
-        {
-            leftqpos(i) = leftPositionSensors[i]->position;
-            leftqvel(i) = leftVelocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf rightqpos;
-        Eigen::VectorXf rightqvel;
-        rightqpos.resize(rightPositionSensors.size());
-        rightqvel.resize(rightVelocitySensors.size());
-
-        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
-        {
-            rightqpos(i) = rightPositionSensors[i]->position;
-            rightqvel(i) = rightVelocitySensors[i]->velocity;
-        }
-
-        // -------------------------------------- compute TCP and object velocity -------------------------------------
-        Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
-        Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
-        Eigen::VectorXf currentTwist(12);
-        currentTwist << currentLeftTwist, currentRightTwist;
-        boxCurrentTwist = pinvGraspMatrixT * currentTwist;
-
-        rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
-        rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
-        rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
-        rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
-        rt2ControlBuffer.commitWrite();
-
-        // pass sensor value to statechart
-        controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
-        controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head(3);
-        controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
-        controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
-        controlInterfaceBuffer.commitWrite();
-
-
-        // --------------------------------------------- get MP target ---------------------------------------------
-        virtualPose = rtGetControlStruct().boxPose;
-        //        Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
-
-        // --------------------------------------------- convert to tcp pose ---------------------------------------------
-        Eigen::Matrix4f tcpTargetPoseLeft = virtualPose;
-        Eigen::Matrix4f tcpTargetPoseRight = virtualPose;
-
-        tcpTargetPoseRight.block<3, 3>(0, 0) = virtualPose.block<3, 3>(0, 0) * fixedLeftRightRotOffset;
-        tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
-        tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
-
-        // --------------------------------------------- Velocity control ---------------------------------------------
-
-        Eigen::Matrix3f diffMatLeft = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).inverse();
-        Eigen::Vector3f errorRPYLeft = VirtualRobot::MathTools::eigen3f2rpy(diffMatLeft);
-        Eigen::Matrix3f diffMatRight = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).inverse();
-        Eigen::Vector3f errorRPYRight = VirtualRobot::MathTools::eigen3f2rpy(diffMatRight);
-
-        Eigen::Vector6f leftTargetVel, rightTargetVel;
-        for (size_t i = 0; i < 3; ++i)
-        {
-            leftTargetVel(i) = KpImpedance(i) * (tcpTargetPoseLeft(i, 3) - currentLeftPose(i, 3)) + KdImpedance(i) * (- currentLeftTwist(i));
-            leftTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYLeft(i) + KdImpedance(i + 3) * (- currentLeftTwist(i + 3));
-            rightTargetVel(i) = KpImpedance(i) * (tcpTargetPoseRight(i, 3) - currentRightPose(i, 3)) + KdImpedance(i) * (- currentRightTwist(i));
-            rightTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYRight(i) + KdImpedance(i + 3) * (- currentRightTwist(i + 3));
-        }
-
-
-
-        Eigen::VectorXf leftJointNullSpaceVel = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel
-                                                + cfg->jointLimitAvoidanceKp * leftTCPController->calculateJointLimitAvoidance();
-        Eigen::VectorXf leftJointTargetVel = calcIK(leftIK, jacobiL, leftTargetVel, leftJointNullSpaceVel);
-
-
-        Eigen::VectorXf rightJointNullSpaceVel = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel
-                + cfg->jointLimitAvoidanceKp * rightTCPController->calculateJointLimitAvoidance();
-        Eigen::VectorXf rightJointTargetVel = calcIK(rightIK, jacobiR, rightTargetVel, rightJointNullSpaceVel);
-
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            float desiredVel = leftJointTargetVel(i);
-            debugOutputData.getWriteBuffer().desired_vels[leftJointNames[i]] = desiredVel;
-
-            if (fabs(desiredVel) > cfg->jointVelLimit || isnan(desiredVel))
-            {
-                desiredVel = 0.0;
-            }
-
-            leftTargets.at(i)->velocity = desiredVel;
-        }
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            float desiredVel = rightJointTargetVel(i);
-            debugOutputData.getWriteBuffer().desired_vels[rightJointNames[i]] = desiredVel;
-
-            if (fabs(desiredVel) > cfg->jointVelLimit  || isnan(desiredVel))
-            {
-                desiredVel = 0.0;
-            }
-
-            rightTargets.at(i)->velocity = desiredVel;
-        }
-
-
-        // --------------------------------------------- debug output ---------------------------------------------
-        debugOutputData.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
-        debugOutputData.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
-        debugOutputData.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
-
-        debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
-
-
-        VirtualRobot::MathTools::Quaternion leftQuat = VirtualRobot::MathTools::eigen4f2quat(currentLeftPose);
-        debugOutputData.getWriteBuffer().leftQuat_w = leftQuat.w;
-        debugOutputData.getWriteBuffer().leftQuat_x = leftQuat.x;
-        debugOutputData.getWriteBuffer().leftQuat_y = leftQuat.y;
-        debugOutputData.getWriteBuffer().leftQuat_z = leftQuat.y;
-
-        debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
-
-        VirtualRobot::MathTools::Quaternion rightQuat = VirtualRobot::MathTools::eigen4f2quat(currentRightPose);
-        debugOutputData.getWriteBuffer().rightQuat_w = rightQuat.w;
-        debugOutputData.getWriteBuffer().rightQuat_x = rightQuat.x;
-        debugOutputData.getWriteBuffer().rightQuat_y = rightQuat.y;
-        debugOutputData.getWriteBuffer().rightQuat_z = rightQuat.y;
-
-
-        debugOutputData.getWriteBuffer().dmpBoxPose_x = virtualPose(0, 3);
-        debugOutputData.getWriteBuffer().dmpBoxPose_y = virtualPose(1, 3);
-        debugOutputData.getWriteBuffer().dmpBoxPose_z = virtualPose(2, 3);
-
-        VirtualRobot::MathTools::Quaternion dmpQuat = VirtualRobot::MathTools::eigen4f2quat(virtualPose);
-        debugOutputData.getWriteBuffer().dmpBoxPose_qx = dmpQuat.x;
-        debugOutputData.getWriteBuffer().dmpBoxPose_qy = dmpQuat.y;
-        debugOutputData.getWriteBuffer().dmpBoxPose_qz = dmpQuat.z;
-        debugOutputData.getWriteBuffer().dmpBoxPose_qw = dmpQuat.w;
-        debugOutputData.commitWrite();
-
-    }
-
-    void NJointBimanualObjLevelVelController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        objectDMP->learnDMPFromFiles(fileNames);
-    }
-
-
-    void NJointBimanualObjLevelVelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard(controllerMutex);
-        objectDMP->setGoalPoseVec(goals);
-        dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
-        dmpGoal(0, 3) = goals[0];
-        dmpGoal(1, 3) = goals[1];
-        dmpGoal(2, 3) = goals[2];
-
-    }
-
-
-    void NJointBimanualObjLevelVelController::runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
-    {
-        while (!controlInterfaceBuffer.updateReadBuffer())
-        {
-            usleep(1000);
-        }
-
-        Eigen::Matrix4f leftPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentLeftPose;
-        Eigen::Matrix4f rightPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentRightPose;
-
-        VirtualRobot::MathTools::Quaternion boxOri = VirtualRobot::MathTools::eigen4f2quat(leftPose);
-        Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
-
-
-        std::vector<double> boxInitialPose;
-        for (size_t i = 0; i < 3; ++i)
-        {
-            boxInitialPose.push_back(boxPosi(i)); //Important: mm -> m
-        }
-        boxInitialPose.push_back(boxOri.w);
-        boxInitialPose.push_back(boxOri.x);
-        boxInitialPose.push_back(boxOri.y);
-        boxInitialPose.push_back(boxOri.z);
-
-        dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
-        dmpGoal(0, 3) = goals[0];
-        dmpGoal(1, 3) = goals[1];
-        dmpGoal(2, 3) = goals[2];
-
-        objectDMP->prepareExecution(boxInitialPose, goals);
-        objectDMP->canVal = timeDuration;
-        objectDMP->config.motionTimeDuration = timeDuration;
-
-
-        finished = false;
-        dmpStarted = true;
-    }
-
-    void NJointBimanualObjLevelVelController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
-    {
-        while (!controlInterfaceBuffer.updateReadBuffer())
-        {
-            usleep(1000);
-        }
-        ARMARX_IMPORTANT << "obj level control: setup dmp ...";
-
-        dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
-        dmpGoal(0, 3) = goals[0];
-        dmpGoal(1, 3) = goals[1];
-        dmpGoal(2, 3) = goals[2];
-
-        objectDMP->prepareExecution(starts, goals);
-        objectDMP->canVal = timeDuration;
-        objectDMP->config.motionTimeDuration = timeDuration;
-
-        finished = false;
-        dmpStarted = true;
-
-        ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
-    }
-
-    void NJointBimanualObjLevelVelController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-        //        LockGuardType guard(controllerMutex);
-        ARMARX_INFO << "setting via points ";
-        objectDMP->setViaPose(u, viapoint);
-    }
-
-    void NJointBimanualObjLevelVelController::removeAllViaPoints(const Ice::Current&)
-    {
-        objectDMP->removeAllViaPoints();
-    }
-
-    void NJointBimanualObjLevelVelController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
-        interface2rtBuffer.commitWrite();
-
-    }
-
-    void NJointBimanualObjLevelVelController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-
-
-    std::vector<float> NJointBimanualObjLevelVelController::getCurrentObjVel(const Ice::Current&)
-    {
-        Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
-        std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
-        return tvelvec;
-    }
-
-
-    void NJointBimanualObjLevelVelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().desired_vels;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-
-        datafields["virtualPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_x);
-        datafields["virtualPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_y);
-        datafields["virtualPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_z);
-
-        datafields["currentPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
-        datafields["currentPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
-        datafields["currentPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
-
-        datafields["leftQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_w);
-        datafields["leftQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_x);
-        datafields["leftQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_y);
-        datafields["leftQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_z);
-
-
-        datafields["currentPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
-        datafields["currentPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
-        datafields["currentPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
-        datafields["rightQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_w);
-        datafields["rightQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_x);
-        datafields["rightQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_y);
-        datafields["rightQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_z);
-
-        datafields["dmpBoxPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
-        datafields["dmpBoxPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
-        datafields["dmpBoxPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
-
-        datafields["dmpBoxPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qx);
-        datafields["dmpBoxPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qy);
-        datafields["dmpBoxPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qz);
-        datafields["dmpBoxPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qw);
-
-        debugObs->setDebugChannel("BimanualForceController", datafields);
-    }
-
-    void NJointBimanualObjLevelVelController::onInitNJointController()
-    {
-
-
-        ARMARX_INFO << "init ...";
-        runTask("NJointBimanualObjLevelVelController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-    }
-
-    void NJointBimanualObjLevelVelController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-    }
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.h
deleted file mode 100644
index c087b7ca0b82b2a9598e29a8457a52f14774b8b4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.h
+++ /dev/null
@@ -1,251 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
-    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelVelController);
-    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelVelControlData);
-
-    class NJointBimanualObjLevelVelControlData
-    {
-    public:
-        // control target from Movement Primitives
-        Eigen::Matrix4f boxPose;
-    };
-
-
-    class NJointBimanualObjLevelVelController :
-        public NJointControllerWithTripleBuffer<NJointBimanualObjLevelVelControlData>,
-        public NJointBimanualObjLevelVelControllerInterface
-    {
-    public:
-        //        using ConfigPtrT = BimanualForceControllerConfigPtr;
-        NJointBimanualObjLevelVelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointCCDMPControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
-        bool isFinished(const Ice::Current&)
-        {
-            return finished;
-        }
-        Eigen::Matrix3f skew(Eigen::Vector3f vec)
-        {
-            Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
-            mat(1, 2) = -vec(0);
-            mat(0, 2) = vec(1);
-            mat(0, 1) = -vec(2);
-            mat(2, 1) = vec(0);
-            mat(2, 0) = -vec(1);
-            mat(1, 0) = vec(2);
-            return mat;
-        }
-
-        //        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
-        void runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-        void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
-        void removeAllViaPoints(const Ice::Current&);
-
-        double getVirtualTime(const Ice::Current&)
-        {
-            return virtualtimer;
-        }
-
-        void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
-        void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
-
-        std::vector<float> getCurrentObjVel(const Ice::Current&);
-
-        void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
-        DoubleSeqSeq getMPWeights(const Ice::Current&);
-
-        void setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&);
-        DoubleSeqSeq getMPRotWeights(const Ice::Current&);
-        Eigen::VectorXf calcIK(VirtualRobot::DifferentialIKPtr ik, const Eigen::MatrixXf& jacobi, const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel);
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitNJointController();
-        void onDisconnectNJointController();
-        void controllerRun();
-
-    private:
-        Eigen::VectorXf targetWrench;
-        struct DebugBufferData
-        {
-            StringFloatDictionary desired_vels;
-
-            float virtualPose_x;
-            float virtualPose_y;
-            float virtualPose_z;
-
-            float currentPoseLeft_x;
-            float currentPoseLeft_y;
-            float currentPoseLeft_z;
-            float leftQuat_w;
-            float leftQuat_x;
-            float leftQuat_y;
-            float leftQuat_z;
-
-            float currentPoseRight_x;
-            float currentPoseRight_y;
-            float currentPoseRight_z;
-            float rightQuat_w;
-            float rightQuat_x;
-            float rightQuat_y;
-            float rightQuat_z;
-
-
-            float dmpBoxPose_x;
-            float dmpBoxPose_y;
-            float dmpBoxPose_z;
-
-            float dmpBoxPose_qx;
-            float dmpBoxPose_qy;
-            float dmpBoxPose_qz;
-            float dmpBoxPose_qw;
-
-        };
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-        struct rt2ControlData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-        };
-        TripleBuffer<rt2ControlData> rt2ControlBuffer;
-
-        struct ControlInterfaceData
-        {
-            Eigen::Matrix4f currentLeftPose;
-            Eigen::Matrix4f currentRightPose;
-            Eigen::Matrix4f currentObjPose;
-            Eigen::Vector3f currentObjVel;
-        };
-
-        TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
-
-        struct Inferface2rtData
-        {
-            Eigen::VectorXf KpImpedance;
-            Eigen::VectorXf KdImpedance;
-        };
-        TripleBuffer<Inferface2rtData> interface2rtBuffer;
-
-
-
-        std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
-        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
-        std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
-        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
-        NJointBimanualObjLevelVelControllerConfigPtr cfg;
-        VirtualRobot::DifferentialIKPtr leftIK;
-        VirtualRobot::DifferentialIKPtr rightIK;
-
-        TaskSpaceDMPControllerPtr objectDMP;
-
-
-
-        double virtualtimer;
-
-        mutable MutexType controllerMutex;
-        mutable MutexType interfaceDataMutex;
-        Eigen::VectorXf leftDesiredJointValues;
-        Eigen::VectorXf rightDesiredJointValues;
-
-        Eigen::Matrix4f leftInitialPose;
-        Eigen::Matrix4f rightInitialPose;
-        Eigen::Matrix4f boxInitialPose;
-
-        Eigen::VectorXf KpImpedance;
-        Eigen::VectorXf KdImpedance;
-        Eigen::VectorXf KpAdmittance;
-        Eigen::VectorXf KdAdmittance;
-        Eigen::VectorXf KmAdmittance;
-        Eigen::VectorXf KmPID;
-
-        Eigen::VectorXf virtualAcc;
-        Eigen::VectorXf virtualVel;
-        Eigen::Matrix4f virtualPose;
-
-        Eigen::Matrix4f sensorFrame2TcpFrameLeft;
-        Eigen::Matrix4f sensorFrame2TcpFrameRight;
-
-        //static compensation
-        float massLeft;
-        Eigen::Vector3f CoMVecLeft;
-        Eigen::Vector3f forceOffsetLeft;
-        Eigen::Vector3f torqueOffsetLeft;
-
-        float massRight;
-        Eigen::Vector3f CoMVecRight;
-        Eigen::Vector3f forceOffsetRight;
-        Eigen::Vector3f torqueOffsetRight;
-
-        //        float knull;
-        //        float dnull;
-
-        std::vector<std::string> leftJointNames;
-        std::vector<std::string> rightJointNames;
-
-        //        float torqueLimit;
-        VirtualRobot::RobotNodeSetPtr leftRNS;
-        VirtualRobot::RobotNodeSetPtr rightRNS;
-        VirtualRobot::RobotNodePtr tcpLeft;
-        VirtualRobot::RobotNodePtr tcpRight;
-
-        CartesianVelocityControllerPtr leftTCPController;
-        CartesianVelocityControllerPtr rightTCPController;
-
-        std::vector<PIDControllerPtr> forcePIDControllers;
-
-        // filter parameters
-        float filterCoeff;
-        Eigen::VectorXf filteredOldValue;
-        bool finished;
-        bool dmpStarted;
-        Eigen::VectorXf ftOffset;
-        Eigen::Matrix4f dmpGoal;
-
-        Eigen::Matrix3f fixedLeftRightRotOffset;
-        Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
-
-    protected:
-        void rtPreActivateController();
-        bool firstLoop;
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/BimanualForceControllersTest.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/BimanualForceControllersTest.cpp
deleted file mode 100644
index eb87f1d1f53f64a0b8fae06287fb08d6bfefb11c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/BimanualForceControllersTest.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    BimanualForceControl::ArmarXObjects::BimanualForceControllers
- * @author     JeffGao ( jianfenggaobit at gmail dot com )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#define BOOST_TEST_MODULE BimanualForceControl::ArmarXLibraries::BimanualForceControllers
-
-#define ARMARX_BOOST_TEST
-
-#include <BimanualForceControl/Test.h>
-#include "../BimanualForceControllers.h"
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
-    BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/CMakeLists.txt
deleted file mode 100644
index 5869452c4979f7bc63b624e9cd00d690efa2d048..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore BimanualForceControllers)
- 
-armarx_add_test(BimanualForceControllersTest BimanualForceControllersTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
deleted file mode 100644
index 4e1caa7a466841f031892e99276e5e22070cfa59..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ /dev/null
@@ -1,73 +0,0 @@
-set(LIB_NAME       RobotAPINJointControllers)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(DMP QUIET)
-find_package(MMMCore QUIET)
-find_package(MMMTools QUIET)
-
-armarx_build_if(DMP_FOUND "DMP not available")
-
-set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
-    RobotUnit dmp
-    )
-
-set(LIB_FILES)
-
-set(LIB_HEADERS)
-
-
-message(STATUS "DMP libraries is ${DMP_LIBRARIES}")
-
-
-list(APPEND LIB_HEADERS
-   DMPController/NJointJSDMPController.h
-#           DMPController/NJointJSPositionDMPController.h
-   DMPController/NJointTSDMPController.h
-   DMPController/NJointCCDMPController.h
-   DMPController/NJointBimanualCCDMPController.h
-   DMPController/NJointBimanualCCDMPVelocityController.h
-   DMPController/NJointTaskSpaceImpedanceDMPController.h
-   DMPController/NJointBimanualForceMPController.h
-   DMPController/NJointPeriodicTSDMPForwardVelController.h
-   DMPController/NJointPeriodicTSDMPCompliantController.h
-   DMPController/NJointAdaptiveWipingController.h
-   DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
-   DMPController/NJointTaskSpaceAdaptiveDMPController.h
-   BimanualForceControllers/NJointBimanualForceController.h
-   BimanualForceControllers/NJointBimanualObjLevelController.h
-   BimanualForceControllers/NJointBimanualObjLevelVelController.h
-   BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
-   BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h
-
-   )
-list(APPEND LIB_FILES
-   DMPController/NJointJSDMPController.cpp
-#           DMPController/NJointJSPositionDMPController.cpp
-   DMPController/NJointTSDMPController.cpp
-   DMPController/NJointCCDMPController.cpp
-   DMPController/NJointBimanualCCDMPController.cpp
-   DMPController/NJointBimanualCCDMPVelocityController.cpp
-   DMPController/NJointTaskSpaceImpedanceDMPController.cpp
-   DMPController/NJointBimanualForceMPController.cpp
-   DMPController/NJointPeriodicTSDMPForwardVelController.cpp
-   DMPController/NJointPeriodicTSDMPCompliantController.cpp
-   DMPController/NJointAdaptiveWipingController.cpp
-   DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
-   DMPController/NJointTaskSpaceAdaptiveDMPController.cpp
-   BimanualForceControllers/NJointBimanualForceController.cpp
-   BimanualForceControllers/NJointBimanualObjLevelController.cpp
-   BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
-   BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
-   BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp
-   )
-
-list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
-
-armarx_add_library("${LIB_NAME}"  "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-if(DMP_FOUND)
-    target_include_directories("${LIB_NAME}" PUBLIC ${DMP_INCLUDE_DIRS})
-endif()
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
deleted file mode 100644
index a79f9120177daef4a3d51d1e42c838927ceb58d5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
+++ /dev/null
@@ -1,768 +0,0 @@
-#include "NJointAdaptiveWipingController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointAdaptiveWipingController> registrationControllerNJointAdaptiveWipingController("NJointAdaptiveWipingController");
-
-    NJointAdaptiveWipingController::NJointAdaptiveWipingController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        useSynchronizedRtRobot();
-        cfg =  NJointAdaptiveWipingControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
-        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
-        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-        for (size_t i = 0; i < rns->getSize(); ++i)
-        {
-            std::string jointName = rns->getNode(i)->getName();
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            velocitySensors.push_back(velocitySensor);
-            positionSensors.push_back(positionSensor);
-        };
-
-        tcp = rns->getTCP();
-        // set tcp controller
-        nodeSetName = cfg->nodeSetName;
-        ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
-        taskSpaceDMPConfig.DMPMode = "Linear";
-        taskSpaceDMPConfig.DMPStyle = "Periodic";
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-
-
-
-        dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
-
-        NJointAdaptiveWipingControllerControlData initData;
-        initData.targetTSVel.resize(6);
-        for (size_t i = 0; i < 6; ++i)
-        {
-            initData.targetTSVel(i) = 0;
-        }
-        reinitTripleBuffer(initData);
-
-        firstRun = true;
-        dmpRunning = false;
-
-
-        ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
-        ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
-        ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
-        ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
-
-        kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
-        dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
-        kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
-        dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
-        kpf = cfg->Kpf;
-        //        forcePID.reset(new PIDController(cfg->kpf, ));
-        knull.setZero(targets.size());
-        dnull.setZero(targets.size());
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            knull(i) = cfg->Knull.at(i);
-            dnull(i) = cfg->Dnull.at(i);
-
-        }
-
-        nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
-        for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
-        {
-            nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
-        }
-
-
-        const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
-        forceSensor = svlf->asA<SensorValueForceTorque>();
-
-
-        ARMARX_CHECK_EQUAL(cfg->ftOffset.size(), 6);
-
-        currentForceOffset.setZero();
-        forceOffset << cfg->ftOffset[0], cfg->ftOffset[1], cfg->ftOffset[2];
-        torqueOffset << cfg->ftOffset[3], cfg->ftOffset[4], cfg->ftOffset[5];
-
-        handMass = cfg->handMass;
-        handCOM << cfg->handCOM[0], cfg->handCOM[1], cfg->handCOM[2];
-
-
-        filteredForce.setZero();
-        filteredTorque.setZero();
-
-        filteredForceInRoot.setZero();
-        filteredTorqueInRoot.setZero();
-
-        UserToRTData initUserData;
-        initUserData.targetForce = 0;
-        user2rtData.reinitAllBuffers(initUserData);
-
-        oriToolDir << 0, 0, 1;
-        gravityInRoot << 0, 0, -9.8;
-
-        qvel_filtered.setZero(targets.size());
-
-        ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
-        ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
-        ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
-        // only for ARMAR-6 (safe-guard)
-        ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
-        ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
-        ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
-
-        ARMARX_CHECK_LESS(cfg->ws_y[0],  cfg->ws_y[1]);
-        ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
-        ARMARX_CHECK_LESS(0,  cfg->ws_y[1]);
-
-        ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
-        ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
-        ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
-
-        adaptK = kpos;
-        lastDiff = 0;
-        changeTimer = 0;
-    }
-
-    void NJointAdaptiveWipingController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-
-
-        RTToControllerData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentPose = tcp->getPoseInRootFrame();
-        initSensorData.currentTwist.setZero();
-        initSensorData.isPhaseStop = false;
-        rt2CtrlData.reinitAllBuffers(initSensorData);
-
-        RTToUserData initInterfaceData;
-        initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
-        initInterfaceData.waitTimeForCalibration = 0;
-        rt2UserData.reinitAllBuffers(initInterfaceData);
-
-        started = false;
-
-        runTask("NJointAdaptiveWipingController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-
-    }
-
-    std::string NJointAdaptiveWipingController::getClassName(const Ice::Current&) const
-    {
-        return "NJointAdaptiveWipingController";
-    }
-
-    void NJointAdaptiveWipingController::controllerRun()
-    {
-        if (!started)
-        {
-            return;
-        }
-
-        if (!dmpCtrl)
-        {
-            return;
-        }
-
-        Eigen::VectorXf targetVels(6);
-        bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
-        if (isPhaseStop)
-        {
-            targetVels.setZero();
-        }
-        else
-        {
-
-            double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
-            Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
-            Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
-
-            LockGuardType guard {controllerMutex};
-            dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
-            targetVels = dmpCtrl->getTargetVelocity();
-
-            debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
-            VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
-            debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
-            debugOutputData.getWriteBuffer().mpcFactor =  dmpCtrl->debugData.mpcFactor;
-            debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
-            debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
-            debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
-            debugOutputData.getWriteBuffer().deltaT = deltaT;
-            debugOutputData.commitWrite();
-        }
-
-        getWriterControlStruct().targetTSVel = targetVels;
-        writeControlStruct();
-
-        dmpRunning = true;
-    }
-
-
-    void NJointAdaptiveWipingController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
-        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
-        rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
-        rt2UserData.commitWrite();
-
-        Eigen::Vector3f currentToolDir;
-        currentToolDir.setZero();
-        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf qpos(positionSensors.size());
-        Eigen::VectorXf qvel(velocitySensors.size());
-        for (size_t i = 0; i < positionSensors.size(); ++i)
-        {
-            qpos(i) = positionSensors[i]->position;
-            qvel(i) = velocitySensors[i]->velocity;
-        }
-
-        qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
-        Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
-
-        Eigen::VectorXf targetVel(6);
-        Eigen::Vector3f axis;
-        axis.setZero();
-        targetVel.setZero();
-        Eigen::Vector3f forceInToolFrame;
-        forceInToolFrame << 0, 0, 0;
-
-        Eigen::Vector3f torqueInToolFrame;
-        torqueInToolFrame << 0, 0, 0;
-
-        float angle = 0;
-        if (firstRun || !dmpRunning)
-        {
-            lastPosition = currentPose.block<2, 1>(0, 3);
-            targetPose = currentPose;
-            firstRun = false;
-            filteredForce.setZero();
-            Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
-
-            Eigen::Matrix3f forceFrameOri =  rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
-            Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
-            Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
-            currentForce = currentForce - handGravity;
-
-            currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
-            origHandOri = currentPose.block<3, 3>(0, 0);
-            toolTransform = origHandOri.transpose();
-            targetVel.setZero();
-        }
-        else
-        {
-            // communicate with DMP controller
-            rtUpdateControlStruct();
-            targetVel = rtGetControlStruct().targetTSVel;
-            targetVel(2) = 0;
-
-            // calculate force
-            Eigen::Vector3f currentForce = forceSensor->force - forceOffset - currentForceOffset;
-
-            Eigen::Matrix3f forceFrameOri =  rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
-            Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
-            Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
-
-            currentForce = currentForce - handGravity;
-            filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * currentForce;
-
-            Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
-            Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
-            currentTorque = currentTorque - handTorque;
-            filteredTorque = (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * currentTorque;
-
-            for (size_t i = 0; i < 3; ++i)
-            {
-                if (fabs(filteredForce(i)) > cfg->forceDeadZone)
-                {
-                    filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
-                }
-                else
-                {
-                    filteredForce(i) = 0;
-                }
-            }
-
-            filteredForceInRoot = forceFrameOri * filteredForce;
-            filteredTorqueInRoot = forceFrameOri * filteredTorque;
-            float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
-
-            Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
-            Eigen::Matrix3f currentToolOri = currentHandOri * toolTransform;
-
-            forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
-            torqueInToolFrame = currentToolOri.transpose() * filteredTorqueInRoot;
-
-            float desiredZVel = kpf * (targetForce - forceInToolFrame(2));
-            targetVel(2) -= desiredZVel;
-            targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
-
-            currentToolDir = currentToolOri * oriToolDir;
-
-            for (int i = 3; i < 6; ++i)
-            {
-                targetVel(i) = 0;
-            }
-
-            // rotation changes
-
-            if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
-            {
-                Eigen::Vector3f toolYDir;
-                toolYDir << 0, 1.0, 0;
-                Eigen::Vector3f toolYDirInRoot = currentToolOri * toolYDir;
-                Eigen::Vector3f projectedFilteredForceInRoot = filteredForceInRoot - filteredForceInRoot.dot(toolYDirInRoot) * toolYDirInRoot;
-                Eigen::Vector3f desiredToolDir = projectedFilteredForceInRoot.normalized();// / projectedFilteredForceInRoot.norm();
-                currentToolDir.normalize();
-
-                axis = currentToolDir.cross(desiredToolDir);
-                axis = axis.normalized();
-                angle = acosf(currentToolDir.dot(desiredToolDir));
-
-
-                if (fabs(angle) < M_PI / 2 && fabs(angle) > cfg->frictionCone)
-                {
-                    // sigmoid function
-                    float adaptedAngularKp = cfg->angularKp / (1 +  exp(10 * (angle - M_PI / 4)));
-                    float angularKp = fmin(adaptedAngularKp, cfg->angularKp);
-
-                    // test axis
-                    Eigen::Vector3f fixedAxis;
-                    if (axis(1) > 0)
-                    {
-                        fixedAxis << 0.0, 1.0, 0.0;
-                    }
-                    else
-                    {
-                        fixedAxis << 0.0, -1.0, 0.0;
-                    }
-                    Eigen::AngleAxisf desiredToolRot(angle - cfg->frictionCone, fixedAxis);
-                    Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
-
-                    Eigen::Vector3f angularDiff = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
-
-                    targetVel.tail(3) = angularKp * angularDiff;
-
-                    //Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
-                    Eigen::Vector3f checkedToolDir =  desiredRotMat * currentToolDir;
-                    checkedToolDir.normalize();
-                }
-            }
-            // integrate for targetPose
-        }
-
-        bool isPhaseStop = false;
-
-        float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
-        if (diff > cfg->phaseDist0)
-        {
-            isPhaseStop = true;
-        }
-
-        float dTf = (float)deltaT;
-
-
-        if (filteredForceInRoot.block<2, 1>(0, 0).norm() > cfg->dragForceDeadZone)
-        {
-            Eigen::Vector2f dragForce = filteredForceInRoot.block<2, 1>(0, 0) - cfg->dragForceDeadZone * filteredForceInRoot.block<2, 1>(0, 0) / filteredForceInRoot.block<2, 1>(0, 0).norm();
-            adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
-            adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
-            lastDiff = diff;
-        }
-        else
-        {
-            adaptK(0) = fmin(adaptK(0) + fabs(dTf * cfg->adaptCoeff), kpos(0));
-            adaptK(1) = fmin(adaptK(1) + fabs(dTf * cfg->adaptCoeff), kpos(1));
-        }
-        adaptK(2) = kpos(2);
-
-        // adaptive control with distance
-
-
-
-
-        targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
-        Eigen::Matrix3f rotMat =   VirtualRobot::MathTools::rpy2eigen3f(dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
-        targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
-
-        if (isPhaseStop)
-        {
-            Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
-            if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
-            {
-                changeTimer += deltaT;
-            }
-            else
-            {
-                lastPosition = currentPose.block<2, 1>(0, 3);
-                changeTimer = 0;
-            }
-
-            if (changeTimer > cfg->changeTimerThreshold)
-            {
-                targetPose(0, 3) = currentPose(0, 3);
-                targetPose(1, 3) = currentPose(1, 3);
-                isPhaseStop = false;
-                changeTimer = 0;
-            }
-        }
-        else
-        {
-            changeTimer = 0;
-        }
-
-
-        targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
-        targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
-
-        targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
-        targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
-
-        targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
-        targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
-
-
-
-        debugRT.getWriteBuffer().currentToolDir = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(currentToolDir);
-        debugRT.getWriteBuffer().targetPose = targetPose;
-        debugRT.getWriteBuffer().globalPose = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
-        debugRT.getWriteBuffer().currentPose = currentPose;
-        debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
-        debugRT.getWriteBuffer().rotationAxis = axis;
-        debugRT.getWriteBuffer().filteredForce = forceInToolFrame;
-        debugRT.getWriteBuffer().globalFilteredForce = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
-        debugRT.getWriteBuffer().targetVel = targetVel;
-        debugRT.getWriteBuffer().adaptK = adaptK;
-        debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
-        debugRT.getWriteBuffer().rotAngle = angle;
-        debugRT.getWriteBuffer().currentTwist = currentTwist;
-        debugRT.getWriteBuffer().filteredTorque = torqueInToolFrame;
-
-
-        debugRT.commitWrite();
-
-        rt2CtrlData.getWriteBuffer().currentPose = currentPose;
-        rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
-        rt2CtrlData.getWriteBuffer().deltaT = deltaT;
-        rt2CtrlData.getWriteBuffer().currentTime += deltaT;
-        rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
-        rt2CtrlData.commitWrite();
-
-        //            Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
-        //            targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
-
-        // inverse dynamic controller
-        jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
-
-
-
-
-        Eigen::Vector6f jointControlWrench;
-        {
-            Eigen::Vector3f targetTCPLinearVelocity;
-            targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
-            Eigen::Vector3f currentTCPLinearVelocity;
-            currentTCPLinearVelocity <<  0.001 * currentTwist(0),  0.001 * currentTwist(1),   0.001 * currentTwist(2);
-            Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
-            Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
-
-            Eigen::Vector3f linearVel = adaptK.cwiseProduct(desiredPosition - currentTCPPosition);
-
-            //            if (isPhaseStop)
-            //            {
-            //                linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
-            //                for (size_t i = 0; i < 3; ++i)
-            //                {
-            //                    linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
-            //                }
-            //            }
-            //            else
-            //            {
-            //                linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
-            //            }
-            Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
-
-            Eigen::Vector3f currentTCPAngularVelocity;
-            currentTCPAngularVelocity << currentTwist(3),   currentTwist(4),  currentTwist(5);
-            Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
-            Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-            Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) + dori.cwiseProduct(- currentTCPAngularVelocity);
-            jointControlWrench <<  tcpDesiredForce, tcpDesiredTorque;
-        }
-
-
-
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
-        Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(nullSpaceJointsVec - qpos) - dnull.cwiseProduct(qvel);
-        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
-        Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
-        // torque filter (maybe)
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            targets.at(i)->torque = jointDesiredTorques(i);
-
-            if (!targets.at(i)->isValid())
-            {
-                targets.at(i)->torque = 0.0f;
-            }
-            else
-            {
-                if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
-                {
-                    targets.at(i)->torque = fabs(cfg->maxJointTorque) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
-                }
-            }
-        }
-
-
-    }
-
-
-    void NJointAdaptiveWipingController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        ARMARX_INFO << "Learning DMP ... ";
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->learnDMPFromFiles(fileNames);
-
-    }
-
-    void NJointAdaptiveWipingController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
-    {
-        ARMARX_INFO << "Learning DMP ... ";
-        ARMARX_CHECK_EXPRESSION(trajectory);
-        TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
-        ARMARX_CHECK_EXPRESSION(dmpTraj);
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->learnDMPFromTrajectory(dmpTraj);
-
-    }
-
-    void NJointAdaptiveWipingController::setSpeed(Ice::Double times, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setSpeed(times);
-    }
-
-
-    void NJointAdaptiveWipingController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setGoalPoseVec(goals);
-    }
-
-    void NJointAdaptiveWipingController::setAmplitude(Ice::Double amp, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setAmplitude(amp);
-    }
-
-    void NJointAdaptiveWipingController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
-    {
-        LockGuardType guard {controlDataMutex};
-        user2rtData.getWriteBuffer().targetForce = targetForce;
-        user2rtData.commitWrite();
-    }
-
-    void NJointAdaptiveWipingController::runDMP(const Ice::DoubleSeq&  goals, Ice::Double tau, const Ice::Current&)
-    {
-        firstRun = true;
-        while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
-        {
-            usleep(100);
-        }
-
-
-        Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-        dmpCtrl->setSpeed(tau);
-
-        ARMARX_IMPORTANT << "run DMP";
-        started = true;
-        dmpRunning = false;
-    }
-
-
-    void NJointAdaptiveWipingController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& debugDrawer, const DebugObserverInterfacePrx& debugObs)
-    {
-        std::string datafieldName;
-        std::string debugName = "Periodic";
-        StringVariantBaseMap datafields;
-
-        Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
-        datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
-        datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
-        datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
-
-        Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
-        datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
-        datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
-        datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
-
-        Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
-        datafields["filteredforceInTool_x"] = new Variant(filteredForce(0));
-        datafields["filteredforceInTool_y"] = new Variant(filteredForce(1));
-        datafields["filteredforceInTool_z"] = new Variant(filteredForce(2));
-
-        Eigen::Vector3f filteredTorque = debugRT.getUpToDateReadBuffer().filteredTorque;
-        datafields["filteredtorqueInTool_x"] = new Variant(filteredTorque(0));
-        datafields["filteredtorqueInTool_y"] = new Variant(filteredTorque(1));
-        datafields["filteredtorqueInTool_z"] = new Variant(filteredTorque(2));
-
-
-        Eigen::Vector3f filteredForceInRoot = debugRT.getUpToDateReadBuffer().filteredForceInRoot;
-        datafields["filteredForceInRoot_x"] = new Variant(filteredForceInRoot(0));
-        datafields["filteredForceInRoot_y"] = new Variant(filteredForceInRoot(1));
-        datafields["filteredForceInRoot_z"] = new Variant(filteredForceInRoot(2));
-
-        Eigen::Vector3f rotationAxis = debugRT.getUpToDateReadBuffer().rotationAxis;
-        datafields["rotationAxis_x"] = new Variant(rotationAxis(0));
-        datafields["rotationAxis_y"] = new Variant(rotationAxis(1));
-        datafields["rotationAxis_z"] = new Variant(rotationAxis(2));
-
-        Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
-        datafields["reactForce_x"] = new Variant(reactForce(0));
-        datafields["reactForce_y"] = new Variant(reactForce(1));
-        datafields["reactForce_z"] = new Variant(reactForce(2));
-
-        Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
-        datafields["targetVel_x"] = new Variant(targetVel(0));
-        datafields["targetVel_y"] = new Variant(targetVel(1));
-        datafields["targetVel_z"] = new Variant(targetVel(2));
-        datafields["targetVel_ro"] = new Variant(targetVel(3));
-        datafields["targetVel_pi"] = new Variant(targetVel(4));
-        datafields["targetVel_ya"] = new Variant(targetVel(5));
-
-        Eigen::VectorXf currentTwist = debugRT.getUpToDateReadBuffer().currentTwist;
-        datafields["currentTwist_x"] = new Variant(currentTwist(0));
-        datafields["currentTwist_y"] = new Variant(currentTwist(1));
-        datafields["currentTwist_z"] = new Variant(currentTwist(2));
-        datafields["currentTwist_ro"] = new Variant(currentTwist(3));
-        datafields["currentTwist_pi"] = new Variant(currentTwist(4));
-        datafields["currentTwist_ya"] = new Variant(currentTwist(5));
-
-
-        Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
-        datafields["adaptK_x"] = new Variant(adaptK(0));
-        datafields["adaptK_y"] = new Variant(adaptK(1));
-
-        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-        datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
-        datafields["rotAngle"] = new Variant(debugRT.getUpToDateReadBuffer().rotAngle);
-
-
-        //        datafields["targetVel_rx"] = new Variant(targetVel(3));
-        //        datafields["targetVel_ry"] = new Variant(targetVel(4));
-        //        datafields["targetVel_rz"] = new Variant(targetVel(5));
-
-        //        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        //        for (auto& pair : values)
-        //        {
-        //            datafieldName = pair.first  + "_" + debugName;
-        //            datafields[datafieldName] = new Variant(pair.second);
-        //        }
-
-        //        auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
-        //        for (auto& pair : currentPose)
-        //        {
-        //            datafieldName = pair.first + "_" + debugName;
-        //            datafields[datafieldName] = new Variant(pair.second);
-        //        }
-
-        //        datafieldName = "canVal_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        //        datafieldName = "mpcFactor_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
-        //        datafieldName = "error_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
-        //        datafieldName = "posError_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
-        //        datafieldName = "oriError_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
-        //        datafieldName = "deltaT_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-        datafieldName = "PeriodicDMP";
-        debugObs->setDebugChannel(datafieldName, datafields);
-
-
-        // draw force;
-        Eigen::Matrix4f globalPose = debugRT.getUpToDateReadBuffer().globalPose;
-        Eigen::Vector3f handPosition = globalPose.block<3, 1>(0, 3);
-        Eigen::Vector3f forceDir = debugRT.getUpToDateReadBuffer().globalFilteredForce;
-
-        debugDrawer->setArrowVisu("Force", "currentForce", new Vector3(handPosition), new Vector3(forceDir), DrawColor {0, 0, 1, 1}, 10 * forceDir.norm(), 3);
-
-        // draw direction of the tool
-        Eigen::Vector3f currentToolDir = debugRT.getUpToDateReadBuffer().currentToolDir;
-        debugDrawer->setArrowVisu("Tool", "Tool", new Vector3(handPosition), new Vector3(currentToolDir), DrawColor {1, 0, 0, 1}, 100, 3);
-        debugDrawer->setPoseVisu("target", "targetPose", new Pose(globalPose));
-
-    }
-
-
-
-    void NJointAdaptiveWipingController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
deleted file mode 100644
index cd07e4be8e3e2b86f13a7dc6acd2aa14b0585241..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
+++ /dev/null
@@ -1,207 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-namespace armarx
-{
-
-
-    TYPEDEF_PTRS_HANDLE(NJointAdaptiveWipingController);
-    TYPEDEF_PTRS_HANDLE(NJointAdaptiveWipingControllerControlData);
-
-    class NJointAdaptiveWipingControllerControlData
-    {
-    public:
-        Eigen::VectorXf targetTSVel;
-    };
-
-    /**
-     * @brief The NJointAdaptiveWipingController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointAdaptiveWipingController :
-        public NJointControllerWithTripleBuffer<NJointAdaptiveWipingControllerControlData>,
-        public NJointAdaptiveWipingControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointAdaptiveWipingControllerConfigPtr;
-        NJointAdaptiveWipingController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointAdaptiveWipingControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
-        void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
-        bool isFinished(const Ice::Current&)
-        {
-            return false;
-        }
-
-        void setSpeed(Ice::Double times, const Ice::Current&);
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void setAmplitude(Ice::Double amp, const Ice::Current&);
-        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
-        void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
-        double getCanVal(const Ice::Current&)
-        {
-            return dmpCtrl->canVal;
-        }
-
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitNJointController();
-        void onDisconnectNJointController();
-        void controllerRun();
-
-    private:
-        struct DebugBufferData
-        {
-            StringFloatDictionary latestTargetVelocities;
-            StringFloatDictionary currentPose;
-            double currentCanVal;
-            double mpcFactor;
-            double error;
-            double phaseStop;
-            double posError;
-            double oriError;
-            double deltaT;
-        };
-
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-
-        struct DebugRTData
-        {
-            Eigen::Matrix4f targetPose;
-            Eigen::Vector3f filteredForce;
-            Eigen::Vector3f filteredForceInRoot;
-            Eigen::Vector3f filteredTorque;
-
-            Eigen::Vector3f rotationAxis;
-
-            Eigen::Vector3f reactForce;
-            Eigen::Vector3f adaptK;
-            Eigen::VectorXf targetVel;
-            Eigen::Matrix4f currentPose;
-            bool isPhaseStop;
-
-            Eigen::Matrix4f globalPose;
-            Eigen::Vector3f globalFilteredForce;
-            Eigen::Vector3f currentToolDir;
-            Eigen::VectorXf currentTwist;
-
-            float rotAngle;
-        };
-        TripleBuffer<DebugRTData> debugRT;
-
-
-
-
-        struct RTToControllerData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-            bool isPhaseStop;
-        };
-        TripleBuffer<RTToControllerData> rt2CtrlData;
-
-        struct RTToUserData
-        {
-            Eigen::Matrix4f currentTcpPose;
-            float waitTimeForCalibration;
-        };
-        TripleBuffer<RTToUserData> rt2UserData;
-
-        struct UserToRTData
-        {
-            float targetForce;
-        };
-        TripleBuffer<UserToRTData> user2rtData;
-
-
-        TaskSpaceDMPControllerPtr dmpCtrl;
-
-        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-        std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
-        // velocity ik controller parameters
-        std::string nodeSetName;
-
-        bool started;
-        bool firstRun;
-        bool dmpRunning;
-
-        VirtualRobot::DifferentialIKPtr ik;
-        VirtualRobot::RobotNodePtr tcp;
-
-        NJointAdaptiveWipingControllerConfigPtr cfg;
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointAdaptiveWipingController>::pointer_type controllerTask;
-        Eigen::Matrix4f targetPose;
-
-        Eigen::Vector3f kpos;
-        Eigen::Vector3f dpos;
-        Eigen::Vector3f kori;
-        Eigen::Vector3f dori;
-        Eigen::VectorXf knull;
-        Eigen::VectorXf dnull;
-        float kpf;
-
-        Eigen::VectorXf nullSpaceJointsVec;
-        const SensorValueForceTorque* forceSensor;
-
-        PIDControllerPtr forcePID;
-
-        Eigen::Vector3f filteredForce;
-        Eigen::Vector3f filteredTorque;
-        Eigen::Vector3f forceOffset;
-        Eigen::Vector3f currentForceOffset;
-
-        Eigen::Vector3f torqueOffset;
-        Eigen::Vector3f currentTorqueOffset;
-        float handMass;
-        Eigen::Vector3f handCOM;
-        Eigen::Vector3f gravityInRoot;
-
-        Eigen::Vector3f filteredForceInRoot;
-        Eigen::Vector3f filteredTorqueInRoot;
-
-        Eigen::Matrix3f toolTransform;
-        Eigen::Vector3f oriToolDir;
-        Eigen::Matrix3f origHandOri;
-        Eigen::VectorXf qvel_filtered;
-
-        Eigen::Vector3f adaptK;
-        float lastDiff;
-        Eigen::Vector2f lastPosition;
-        double changeTimer;
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
deleted file mode 100644
index 65835f6ac7c69d3dd16ae361095bbdecfd848bc1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
+++ /dev/null
@@ -1,1147 +0,0 @@
-#include "NJointAnomalyDetectionAdaptiveWipingController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointAnomalyDetectionAdaptiveWipingController> registrationControllerNJointAnomalyDetectionAdaptiveWipingController("NJointAnomalyDetectionAdaptiveWipingController");
-
-    NJointAnomalyDetectionAdaptiveWipingController::NJointAnomalyDetectionAdaptiveWipingController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        useSynchronizedRtRobot();
-        cfg =  NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
-        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
-        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-        for (size_t i = 0; i < rns->getSize(); ++i)
-        {
-            std::string jointName = rns->getNode(i)->getName();
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            velocitySensors.push_back(velocitySensor);
-            positionSensors.push_back(positionSensor);
-        };
-
-        useDMPInGlobalFrame = cfg->useDMPInGlobalFrame;
-
-        tcp = rns->getTCP();
-        // set tcp controller
-        nodeSetName = cfg->nodeSetName;
-        ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
-        taskSpaceDMPConfig.DMPMode = "Linear";
-        taskSpaceDMPConfig.DMPStyle = "Periodic";
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-
-        lastCanVal = cfg->timeDuration;
-
-        dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
-
-        NJointAnomalyDetectionAdaptiveWipingControllerControlData initData;
-        initData.targetTSVel.resize(6);
-        for (size_t i = 0; i < 6; ++i)
-        {
-            initData.targetTSVel(i) = 0;
-        }
-        initData.targetTSPose = tcp->getPoseInRootFrame();
-        reinitTripleBuffer(initData);
-
-        firstRun = true;
-        dmpRunning = false;
-
-        // anomaly detection
-        velocityHorizon = cfg->velocityHorizon;
-
-        // friction estimation
-        frictionHorizon = cfg->frictionHorizon;
-        estimatedFriction << 0.0, 0.0;
-        lastForceInToolXY.setZero();
-
-        ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
-        ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
-        ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
-        ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
-
-        kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
-        dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
-        kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
-        dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
-        isForceCtrlInForceDir = cfg->isForceCtrlInForceDir;
-        isForceControlEnabled = cfg->isForceControlEnabled;
-        isRotControlEnabled = cfg->isRotControlEnabled;
-        isTorqueControlEnabled = cfg->isTorqueControlEnabled;
-        isLCRControlEnabled = cfg->isLCRControlEnabled;
-        forcePID.reset(new PIDController(cfg->pidForce[0], cfg->pidForce[1], cfg->pidForce[2], cfg->pidForce[3]));
-        rotPID.reset(new PIDController(cfg->pidRot[0], cfg->pidRot[1], cfg->pidRot[2], cfg->pidRot[3]));
-        torquePID.reset(new PIDController(cfg->pidTorque[0], cfg->pidTorque[1], cfg->pidTorque[2], cfg->pidTorque[3]));
-        lcrPID.reset(new PIDController(cfg->pidLCR[0], cfg->pidLCR[1], cfg->pidLCR[2], cfg->pidLCR[3]));
-        adaptKpForce = cfg->pidForce[0];
-        adaptKpRot = cfg->pidRot[0];
-
-        knull.setZero(targets.size());
-        dnull.setZero(targets.size());
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            knull(i) = cfg->Knull.at(i);
-            dnull(i) = cfg->Dnull.at(i);
-        }
-
-        nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
-        for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
-        {
-            nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
-        }
-
-
-        const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
-        forceSensor = svlf->asA<SensorValueForceTorque>();
-
-
-        ARMARX_CHECK_EQUAL(cfg->ftOffset.size(), 6);
-
-        currentForceOffset.setZero();
-        forceOffset << cfg->ftOffset[0], cfg->ftOffset[1], cfg->ftOffset[2];
-        torqueOffset << cfg->ftOffset[3], cfg->ftOffset[4], cfg->ftOffset[5];
-
-        handMass = cfg->handMass;
-        handCOM << cfg->handCOM[0], cfg->handCOM[1], cfg->handCOM[2];
-
-
-        filteredForce.setZero();
-        filteredTorque.setZero();
-        filteredFTCommand.setZero();
-        filteredForceInRoot.setZero();
-        filteredTorqueInRoot.setZero();
-        targetFTInToolFrame.setZero();
-
-        UserToRTData initUserData;
-        initUserData.targetForce = 0;
-        user2rtData.reinitAllBuffers(initUserData);
-
-        oriToolDir << 0, 0, 1;
-        gravityInRoot << 0, 0, -9.8;
-
-        qvel_filtered.setZero(targets.size());
-
-        ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
-        ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
-        ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
-        // only for ARMAR-6 (safe-guard)
-        ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
-        ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
-        ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
-
-        ARMARX_CHECK_LESS(cfg->ws_y[0],  cfg->ws_y[1]);
-        ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
-        ARMARX_CHECK_LESS(0,  cfg->ws_y[1]);
-
-        ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
-        ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
-        ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
-
-        adaptK = kpos;
-        adaptD = dpos;
-        adaptKOri = kori;
-        adaptDOri = dori;
-        adaptKNull = knull;
-        adaptDNull = dnull;
-        lastDiff = 0;
-        changeTimer = 0;
-
-        abnormalFlag = false;
-        lastAbnormalFlag = false;
-        positionOffset.setZero();
-
-        //        toolToFTSensorLink =
-        //                rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3,1>(0, 3) -
-        //                tcp->getPoseInRootFrame().block<3,1>(0,3)
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-
-
-        RTToControllerData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentPose = tcp->getPoseInRootFrame();
-        initSensorData.currentTwist.setZero();
-        initSensorData.isPhaseStop = false;
-        rt2CtrlData.reinitAllBuffers(initSensorData);
-
-        RTToUserData initInterfaceData;
-        initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
-        initInterfaceData.waitTimeForCalibration = 0;
-        rt2UserData.reinitAllBuffers(initInterfaceData);
-
-        started = false;
-
-        runTask("NJointAnomalyDetectionAdaptiveWipingController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-
-    }
-
-    std::string NJointAnomalyDetectionAdaptiveWipingController::getClassName(const Ice::Current&) const
-    {
-        return "NJointAnomalyDetectionAdaptiveWipingController";
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::controllerRun()
-    {
-        if (!started)
-        {
-            return;
-        }
-
-        if (!dmpCtrl)
-        {
-            return;
-        }
-
-        Eigen::VectorXf targetVels(6);
-        Eigen::Matrix4f targetDMPPose;
-        bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
-        if (isPhaseStop)
-        {
-            targetVels.setZero();
-            targetDMPPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
-        }
-        else
-        {
-
-            double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
-            Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
-            Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
-
-            LockGuardType guard {controllerMutex};
-            dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
-            targetVels = dmpCtrl->getTargetVelocity();
-            targetDMPPose = dmpCtrl->getTargetPoseMat();
-
-            debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
-            VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
-            debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
-            debugOutputData.getWriteBuffer().mpcFactor =  dmpCtrl->debugData.mpcFactor;
-            debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
-            debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
-            debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
-            debugOutputData.getWriteBuffer().deltaT = deltaT;
-            debugOutputData.commitWrite();
-        }
-        getWriterControlStruct().canVal = dmpCtrl->canVal;
-        getWriterControlStruct().targetTSVel = targetVels;
-        getWriterControlStruct().targetTSPose = targetDMPPose;
-        writeControlStruct();
-
-        dmpRunning = true;
-    }
-
-
-    void NJointAnomalyDetectionAdaptiveWipingController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-        float dTf = (float)deltaT;
-
-        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
-
-
-        Eigen::Vector3f currentToolDir;
-        currentToolDir.setZero();
-        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf qpos(positionSensors.size());
-        Eigen::VectorXf qvel(velocitySensors.size());
-        for (size_t i = 0; i < positionSensors.size(); ++i)
-        {
-            qpos(i) = positionSensors[i]->position;
-            qvel(i) = velocitySensors[i]->velocity;
-        }
-
-        qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
-        Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
-
-        velocityHorizonList.push_back(currentTwist);
-        if (velocityHorizonList.size() > velocityHorizon)
-        {
-            velocityHorizonList.pop_front();
-        }
-
-        Eigen::VectorXf targetVel(6);
-        Eigen::Vector3f axis;
-        Eigen::Vector3f forceInToolFrame;
-        Eigen::Vector3f torqueInToolFrame;
-        Eigen::Vector6f targetFTInRootFrame;
-        Eigen::Vector3f velPInToolFrame;
-        targetVel.setZero();
-        axis.setZero();
-        forceInToolFrame.setZero();
-        torqueInToolFrame.setZero();
-        targetFTInRootFrame.setZero();
-        velPInToolFrame.setZero();
-        float angle = 0;
-        bool isPhaseStop = false;
-
-        if (firstRun || !dmpRunning)
-        {
-            initHandPose = currentPose;
-            lastPosition = currentPose.block<2, 1>(0, 3);
-            targetPose = currentPose;
-            firstRun = false;
-            filteredForce.setZero();
-            Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
-
-            Eigen::Matrix3f forceFrameOri =  rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
-            Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
-            Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
-            currentForce = currentForce - handGravity;
-
-            currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
-            origHandOri = currentPose.block<3, 3>(0, 0);
-            toolTransform = origHandOri.transpose();
-            targetVel.setZero();
-        }
-        else
-        {
-            rtUpdateControlStruct();
-            targetVel = rtGetControlStruct().targetTSVel;
-
-
-            Eigen::Matrix3f currentToolOri = currentPose.block<3, 3>(0, 0) * toolTransform;
-
-            /* -------------------------- get target vel from dmp thread --------------------------------- */
-            targetVel(2) = 0;
-            targetVel.head(3) = currentToolOri * targetVel.head(3);
-            targetVel.tail(3) = currentToolOri * targetVel.tail(3);
-
-            double canVal = rtGetControlStruct().canVal;
-            if (canVal - lastCanVal > 0.9 * cfg->timeDuration)
-            {
-                wipingCounter++;
-                mu = 1.0;
-            }
-            lastCanVal = canVal;
-
-            /* -------------------------- force feedback, filter and transform frame --------------------------------- */
-            Eigen::Vector3f currentForce = forceSensor->force - forceOffset - currentForceOffset;
-
-            Eigen::Matrix3f forceFrameOri =  rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
-            Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
-            Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
-
-            currentForce = currentForce - handGravity;
-            filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * currentForce;
-
-            Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
-            Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
-            currentTorque = currentTorque - handTorque;
-            filteredTorque = (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * currentTorque;
-
-            Eigen::Vector3f forceInRootForFricEst = forceFrameOri * filteredForce;
-            Eigen::Vector3f forceInToolForFricEst = currentToolOri.transpose() * forceInRootForFricEst;
-
-            for (size_t i = 0; i < 3; ++i)
-            {
-                if (fabs(filteredForce(i)) > cfg->forceDeadZone)
-                {
-                    filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
-                }
-                else
-                {
-                    filteredForce(i) = 0;
-                }
-            }
-
-            filteredForceInRoot = forceFrameOri * filteredForce;
-            filteredTorqueInRoot = forceFrameOri * filteredTorque;
-            float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
-
-            forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
-            // TODO this is wrong
-            torqueInToolFrame = currentToolOri.transpose() * filteredTorqueInRoot;
-            velPInToolFrame = currentToolOri.transpose() * currentTwist.head(3);
-            //            Eigen::Vector3f velRInToolFrame = currentToolOri.transpose() * currentTwist.tail(3);
-
-
-            /* -------------------------- Drag Force Adaptation --------------------------------- */
-            //            Eigen::Vector3f dragForce = filteredForceInRoot - cfg->dragForceDeadZone * filteredForceInRoot / filteredForceInRoot.norm();
-            if (abnormalFlag == true && filteredForceInRoot.norm() > cfg->dragForceDeadZone)
-            {
-                //                adaptKpForce = fmax(adaptKpForce - dTf * cfg->decreaseKpForceCoeff, 0);
-                //                adaptKpRot = fmax(adaptKpRot - dTf * cfg->decreaseKpRotCoeff, 0);
-                adaptKpForce *= cfg->adaptRateDecrease;
-                adaptKpRot *= cfg->adaptRateDecreaseRot;
-                forcePID->Kp = adaptKpForce;
-                torquePID->Kp = adaptKpRot;
-                //                adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
-                //                adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
-                //                adaptK(2) = fmax(adaptK(2) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
-                //                adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff, 0);
-                //                adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff, 0);
-                //                adaptK(2) = fmax(adaptK(2) - dTf * cfg->adaptForceCoeff, 0);
-                adaptK *= cfg->adaptRateDecrease;
-                adaptD *= cfg->adaptRateDecrease;
-                if (cfg->isAdaptOriImpEnabled)
-                {
-                    adaptKOri *= cfg->adaptRateDecrease;
-                    adaptDOri *= cfg->adaptRateDecrease;
-                }
-                adaptKNull *= cfg->adaptRateDecrease;
-                adaptDNull *= cfg->adaptRateDecrease;
-
-                positionOffset.setZero();
-                forcePID->reset();
-
-                //                lastDiff = diff;
-            }
-            else
-            {
-                adaptKpForce = fmin(adaptKpForce + dTf * cfg->increaseKpForceCoeff, cfg->pidForce[0]);
-                adaptKpRot = fmin(adaptKpRot + dTf * cfg->increaseKpRotCoeff, cfg->pidRot[0]);
-                //                adaptKpForce *= cfg->adaptRateIncrease;
-                //                adaptKpRot *= cfg->adaptRateIncrease;
-                forcePID->Kp = adaptKpForce;
-                torquePID->Kp = adaptKpRot;
-                for (int i = 0; i < 3; i++)
-                {
-                    adaptK(i) = fmin(adaptK(i) + fabs(dTf * cfg->adaptCoeff), kpos(i));
-                    adaptD(i) = fmin(adaptD(i) + fabs(dTf * cfg->adaptCoeffKdImpIncrease), dpos(i));
-                    if (cfg->isAdaptOriImpEnabled)
-                    {
-                        adaptKOri(i) = fmin(adaptKOri(i) + fabs(dTf * cfg->increaseKpOriCoeff), kori(i));
-                        adaptDOri(i) = fmin(adaptDOri(i) + fabs(dTf * cfg->increaseKdOriCoeff), dori(i));
-                    }
-                }
-                for (size_t i = 0; i < targets.size(); i++)
-                {
-                    adaptKNull(i) = fmin(adaptKNull(i) + fabs(dTf * cfg->increaseKpNullCoeff), knull(i));
-                    adaptDNull(i) = fmin(adaptDNull(i) + fabs(dTf * cfg->increaseKdNullCoeff), dnull(i));
-                }
-
-                //                adaptK *= cfg->adaptRateIncrease;
-                //                adaptD *= cfg->adaptRateIncrease;
-            }
-
-            if (!isContactedOnce && fabs(forceInToolFrame(2)) > targetForce)
-            {
-                isContactedOnce = true;
-            }
-            if (cfg->loseContactDetectionEnabled && isContactedOnce)
-            {
-                if (abnormalFlag && !lastAbnormalFlag)
-                {
-                    startLoseContactDetection = true;
-                    loseContactCounter = 0;
-                    forceIntegral = 0;
-
-                }
-                if (startLoseContactDetection && loseContactCounter < cfg->loseContactCounterMax)
-                {
-                    forceIntegral += forceInToolFrame.norm() * deltaT;
-                    loseContactCounter ++;
-                }
-                if (loseContactCounter >= cfg->loseContactCounterMax && forceIntegral < cfg->loseContactForceIntThreshold)
-                {
-                    isLoseContact = true;
-                }
-                lastAbnormalFlag = abnormalFlag;
-                if (isLoseContact)
-                {
-                    adaptK *= 0.0;
-                    adaptD *= 0.0;
-                    adaptKOri *= 0.0;
-                    adaptDOri *= 0.0;
-                    adaptKNull *= 0.0;
-                    adaptDNull *= 0.0;
-                }
-            }
-
-            //            adaptK(2) = kpos(2);
-            /* -------------------------- friction estimation --------------------------------- */
-
-            Eigen::Vector2f v_xy;
-            Eigen::Vector2f f_xy;
-            v_xy << velPInToolFrame(0), velPInToolFrame(1);
-            f_xy << forceInToolForFricEst(0), forceInToolForFricEst(1);
-            f_xy = cfg->fricEstiFilter * f_xy + (1 - cfg->fricEstiFilter) * lastForceInToolXY;
-            lastForceInToolXY = f_xy;
-
-            if (wipingCounter > 0)
-            {
-                if (v_xy.norm() > cfg->velNormThreshold && fabs(forceInToolForFricEst(2) - targetForce) < 0.5 * targetForce)
-                {
-                    recordFrictionNorm.push_back(f_xy.norm());
-                    recordForceNormToSurface.push_back(forceInToolForFricEst(2));
-                }
-                if (recordFrictionNorm.size() > frictionHorizon)
-                {
-                    recordFrictionNorm.pop_front();
-                    recordForceNormToSurface.pop_front();
-                    float dotProduct = 0.0;
-                    float normSquare = 0.0;
-                    for (size_t i = 0; i < recordFrictionNorm.size(); i++)
-                    {
-                        dotProduct += (recordFrictionNorm[i] * recordForceNormToSurface[i]);
-                        normSquare += (recordForceNormToSurface[i] * recordForceNormToSurface[i]);
-                    }
-                    if (normSquare > 0)
-                    {
-                        float mu_tmp = dotProduct / normSquare;
-                        if (mu_tmp > 0)
-                        {
-                            mu = fmax(fmin(mu, mu_tmp), safeFrictionConeLowerLimit);
-                        }
-                    }
-                    if (v_xy.norm() > cfg->velNormThreshold)
-                    {
-                        estimatedFriction = - v_xy * mu * forceInToolForFricEst(2) / v_xy.norm();
-                    }
-                }
-            }
-
-            /* -------------------------- Force Regulation and Torque PID Controller --------------------------------- */
-
-            if (isForceCtrlInForceDir)
-            {
-                forcePID->update(deltaT, forceInToolFrame.norm(), targetForce);
-            }
-            else
-            {
-                forcePID->update(deltaT, forceInToolFrame(2), targetForce);
-            }
-            torquePID->update(deltaT, torqueInToolFrame(1), 0.0);
-
-            /* -------------------------- Rotation PID Controller --------------------------------- */
-
-            currentToolDir = currentToolOri * oriToolDir;
-            for (int i = 3; i < 6; ++i)
-            {
-                targetVel(i) = 0;
-            }
-            float frictionCone;
-            if (cfg->frictionCone < 0.0)
-            {
-                frictionCone = atan(mu);
-
-            }
-            else
-            {
-                frictionCone = cfg->frictionCone;
-            }
-            float adaptedAngularKp = 0.0;
-            Eigen::Vector3f angularDiff;
-            angularDiff.setZero();
-            // rotation changes
-            if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
-            {
-                Eigen::Vector3f toolYDir;
-                toolYDir << 0, 1.0, 0;
-                Eigen::Vector3f toolYDirInRoot = currentToolOri * toolYDir;
-                Eigen::Vector3f projectedFilteredForceInRoot = filteredForceInRoot - filteredForceInRoot.dot(toolYDirInRoot) * toolYDirInRoot;
-                Eigen::Vector3f desiredToolDir = projectedFilteredForceInRoot.normalized();// / projectedFilteredForceInRoot.norm();
-                currentToolDir.normalize();
-
-                axis = currentToolDir.cross(desiredToolDir);
-                axis = axis.normalized();
-                angle = acosf(currentToolDir.dot(desiredToolDir));
-
-                int sign = 1;
-                if (axis(1) < 0)
-                {
-                    sign = -1;
-                }
-
-                if (fabs(angle) < M_PI / 2 && fabs(angle) > frictionCone)
-                {
-                    // sigmoid function
-                    adaptedAngularKp = cfg->pidRot[0] / (1 +  exp(10 * (angle - cfg->rotAngleSigmoid)));
-                    adaptedAngularKp = fmin(adaptedAngularKp, cfg->pidRot[0]);
-                    rotPID->Kp = adaptedAngularKp;
-                    angle -= frictionCone;
-                    angle *= sign;
-                }
-                else
-                {
-                    angle = 0.0;
-                    rotPID->Kp = cfg->pidRot[0];
-                }
-            }
-            rotPID->update(deltaT, angle, 0.0);
-
-            /* -------------------------- Lose Contact Recover PID Controller --------------------------------- */
-
-            //            if (forceInToolFrame(2) > loseContactRatio * targetForce)
-            //            {
-            //                makingContactCounter++;
-            //                if (makingContactCounter > 100)
-            //                {
-            //                    isMakingContact = true;
-            //                    isLCREnabled = false;
-            //                }
-            //                else
-            //                {
-            //                    isMakingContact = false;
-            //                }
-            //            }
-            //            if (!isContactedOnce && isMakingContact)
-            //            {
-            //                isContactedOnce = true;
-            //            }
-            //            Eigen::Vector3f compensationAxis;
-            //            compensationAxis.setZero();
-            //            if (isContactedOnce && fabs(velPInToolFrame(2)) > 10 && frictionCone < 1.0)
-            //            {
-            //                makingContactCounter = 0;
-            //                Eigen::Vector3f v;
-            //                v << velPInToolFrame(0), 0.0, 0.0;
-            //                Eigen::Vector3f f;
-            //                f << 0.0, 0.0, targetForce;
-            //                compensationAxis = f.cross(v);
-            //                if (compensationAxis.norm() > 0)
-            //                {
-            //                    compensationAxis.normalized();
-            //                }
-            //                else
-            //                {
-            //                    compensationAxis.setZero();
-            //                }
-            //                forceControlGate *= 0.5;
-            //                isLCREnabled = true;
-            //                lcrCounter -= 1;
-            //            }
-            //            else
-            //            {
-            //                forceControlGate = 1.0;
-            //                // TODO: restart vmp controller
-            //            }
-            //            float velInForceDir = 0.0;
-            //            if (lcrCounter < 500)
-            //            {
-            //                velInForceDir = fabs(velPInToolFrame(2));
-            //                lcrCounter -= 1;
-            //                if (lcrCounter == 0)
-            //                {
-            //                    lcrCounter = 500;
-            //                }
-            //            }
-            //            lcrPID->update(deltaT, velInForceDir, 0.0);
-
-            /* -------------------------- VMP Phase Stop --------------------------------- */
-
-            float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
-            if (diff > cfg->phaseDist0)
-            {
-                isPhaseStop = true;
-            }
-
-
-            /* -------------------------- get PID control commands --------------------------------- */
-            //            if (isLCRControlEnabled)
-            //            {
-            //                //                targetFTInToolFrame.tail(3) += (float)lcrPID->getControlValue() * compensationAxis;
-            ////                targetVel.tail(3) += (float)lcrPID->getControlValue() * compensationAxis;
-            //            }
-
-            if (isForceControlEnabled)
-            {
-                if (isForceCtrlInForceDir)
-                {
-                    float forcePIDVel = -(float)forcePID->getControlValue();
-                    Eigen::Vector3f targetVelInTool;
-                    if (forceInToolFrame.norm() < 1e-4)
-                    {
-                        targetVelInTool << 0, 0, forcePIDVel;
-                    }
-                    else
-                    {
-                        targetVelInTool = forceInToolFrame / forceInToolFrame.norm() * forcePIDVel;
-                    }
-                    targetVel.head(3) += currentToolOri * targetVelInTool;
-                }
-                else
-                {
-                    //                targetFTInToolFrame(2) -= (float)forcePID->getControlValue() * forceControlGate;
-                    Eigen::Vector3f localVel;
-                    localVel << 0, 0, -(float)forcePID->getControlValue();
-                    positionOffset += currentToolOri * localVel * deltaT;
-
-                    targetVel(2) -= (float)forcePID->getControlValue();
-                    targetVel.head(3) = currentToolOri * targetVel.head(3);
-                }
-            }
-            else
-            {
-                positionOffset.setZero();
-            }
-
-            if (isRotControlEnabled)
-            {
-                //                targetFTInToolFrame(4) -= (float)rotPID->getControlValue();
-                //                targetVel.tail(3) = adaptedAngularKp * angularDiff;
-                targetVel(4) -= (float)rotPID->getControlValue();
-            }
-            if (isTorqueControlEnabled)
-            {
-                //                targetFTInToolFrame(4) -= (float)torquePID->getControlValue();
-                targetVel(4) += (float)torquePID->getControlValue();
-            }
-
-            //            targetFTInRootFrame.head(3) = currentToolOri * targetFTInToolFrame.head(3);
-            //            targetFTInRootFrame.tail(3) = currentToolOri * targetFTInToolFrame.tail(3);
-        }
-
-
-        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
-        rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head(3);
-        rt2UserData.getWriteBuffer().forceOutput = filteredForceInRoot;
-        rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
-        rt2UserData.commitWrite();
-
-        /* -------------------------- Integrate Target Velocity 2 Target Pose --------------------------------- */
-
-        if (useDMPInGlobalFrame)
-        {
-            targetPose = rtGetControlStruct().targetTSPose;
-            targetPose.block<3, 1>(0, 3) += positionOffset;
-        }
-        else
-        {
-            targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
-            Eigen::Matrix3f rotMat =   VirtualRobot::MathTools::rpy2eigen3f(dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
-            targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
-
-            if (isPhaseStop)
-            {
-                Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
-                if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
-                {
-                    changeTimer += deltaT;
-                }
-                else
-                {
-                    lastPosition = currentPose.block<2, 1>(0, 3);
-                    changeTimer = 0;
-                }
-
-                if (changeTimer > cfg->changeTimerThreshold)
-                {
-                    targetPose(0, 3) = currentPose(0, 3);
-                    targetPose(1, 3) = currentPose(1, 3);
-                    isPhaseStop = false;
-                    changeTimer = 0;
-                }
-            }
-            else
-            {
-                changeTimer = 0;
-            }
-        }
-
-        targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
-        targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
-
-        targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
-        targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
-
-        targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
-        targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
-
-        /* -------------------------- Force/Torque Impedance Controller --------------------------------- */
-
-        // inverse dynamic controller
-        jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
-
-        Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).transpose();
-        Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-        Eigen::Vector6f taskFTControlCommand;
-
-        //        for (int i = 0; i < 3; i++)
-        //        {
-        //            adaptD[i] = (float)2 * sqrt(adaptK[i]);
-        //        }
-        taskFTControlCommand.head(3) = adaptK.cwiseProduct(targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) * 0.001 +
-                                       adaptD.cwiseProduct(- currentTwist.head(3)) * 0.001 +
-                                       targetFTInRootFrame.head(3);
-        taskFTControlCommand.tail(3) = kori.cwiseProduct(rpy) +
-                                       dori.cwiseProduct(- currentTwist.tail(3)) +
-                                       targetFTInRootFrame.tail(3);
-
-        filteredFTCommand = cfg->ftCommandFilter * taskFTControlCommand + (1 - cfg->ftCommandFilter) * filteredFTCommand;
-
-        /* -------------------------- NullSpace and Joint Torque Controller --------------------------------- */
-
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
-        Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(nullSpaceJointsVec - qpos) - dnull.cwiseProduct(qvel);
-        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
-        Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * filteredFTCommand + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-        //        if (forceInToolFrame.norm() > cfg->maxInteractionForce)
-        //        {
-        //            jointDesiredTorques.setZero();
-        //        }
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            targets.at(i)->torque = jointDesiredTorques(i);
-
-            if (!targets.at(i)->isValid())
-            {
-                targets.at(i)->torque = 0.0f;
-            }
-            else
-            {
-                if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
-                {
-                    targets.at(i)->torque = fabs(cfg->maxJointTorque) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
-                }
-            }
-        }
-
-        /* -------------------------- Communication --------------------------------- */
-
-        debugRT.getWriteBuffer().currentToolDir = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(currentToolDir);
-        debugRT.getWriteBuffer().targetPose = targetPose;
-        debugRT.getWriteBuffer().globalPose = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
-        debugRT.getWriteBuffer().currentPose = currentPose;
-        debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
-        debugRT.getWriteBuffer().rotationAxis = axis;
-        debugRT.getWriteBuffer().filteredForce = forceInToolFrame;
-        debugRT.getWriteBuffer().globalFilteredForce = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
-        debugRT.getWriteBuffer().targetVel = targetVel;
-        debugRT.getWriteBuffer().adaptK = adaptK;
-        debugRT.getWriteBuffer().adaptD = adaptD;
-        debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
-        debugRT.getWriteBuffer().rotAngle = angle;
-        debugRT.getWriteBuffer().currentTwist = currentTwist;
-        debugRT.getWriteBuffer().filteredTorque = torqueInToolFrame;
-        debugRT.getWriteBuffer().wipingCounter = wipingCounter;
-        debugRT.getWriteBuffer().mu = mu;
-        debugRT.getWriteBuffer().estimatedFriction = estimatedFriction;
-        debugRT.getWriteBuffer().frictionInToolXY = lastForceInToolXY;
-        debugRT.getWriteBuffer().velPInTool = velPInToolFrame;
-        debugRT.getWriteBuffer().kpForcePID = adaptKpForce;
-        debugRT.getWriteBuffer().kpRotPID = adaptKpRot;
-        debugRT.getWriteBuffer().loseContactForceIntegral = forceIntegral;
-
-
-        debugRT.commitWrite();
-
-        rt2CtrlData.getWriteBuffer().currentPose = currentPose;
-        rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
-        rt2CtrlData.getWriteBuffer().deltaT = deltaT;
-        rt2CtrlData.getWriteBuffer().currentTime += deltaT;
-        rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
-        rt2CtrlData.commitWrite();
-
-    }
-
-
-    void NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        ARMARX_INFO << "Learning DMP ... ";
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->learnDMPFromFiles(fileNames);
-
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
-    {
-        ARMARX_INFO << "Learning DMP ... ";
-        ARMARX_CHECK_EXPRESSION(trajectory);
-        TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
-        ARMARX_CHECK_EXPRESSION(dmpTraj);
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->learnDMPFromTrajectory(dmpTraj);
-
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::setSpeed(Ice::Double times, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setSpeed(times);
-    }
-
-
-    void NJointAnomalyDetectionAdaptiveWipingController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setGoalPoseVec(goals);
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::setAmplitude(Ice::Double amp, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setAmplitude(amp);
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
-    {
-        LockGuardType guard {controlDataMutex};
-        user2rtData.getWriteBuffer().targetForce = targetForce;
-        user2rtData.commitWrite();
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::runDMP(const Ice::DoubleSeq&  goals, Ice::Double tau, const Ice::Current&)
-    {
-        firstRun = true;
-        while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
-        {
-            usleep(100);
-        }
-
-
-        Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-        dmpCtrl->setSpeed(tau);
-
-        ARMARX_IMPORTANT << "run DMP";
-        started = true;
-        dmpRunning = false;
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::setTrigerAbnormalEvent(bool abnormal, const Ice::Current&)
-    {
-        abnormalFlag = abnormal;
-    }
-
-    std::vector<float> NJointAnomalyDetectionAdaptiveWipingController::getAnomalyInput(const Ice::Current&)
-    {
-        Eigen::Matrix4f tpos = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
-        Eigen::Vector3f tvel = rt2UserData.getUpToDateReadBuffer().tcpTranslVel;
-        std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2), tpos(0, 3) / 1000, tpos(1, 3) / 1000, tpos(2, 3) / 1000};
-        return tvelvec;
-    }
-
-    std::vector<float> NJointAnomalyDetectionAdaptiveWipingController::getAnomalyOutput(const Ice::Current&)
-    {
-        Eigen::Vector3f force = rt2UserData.getUpToDateReadBuffer().forceOutput;
-        std::vector<float> forceVec = {force(0), force(1), force(2)};
-        return forceVec;
-    }
-
-
-    void NJointAnomalyDetectionAdaptiveWipingController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& debugDrawer, const DebugObserverInterfacePrx& debugObs)
-    {
-        std::string datafieldName;
-        std::string debugName = "Periodic";
-        StringVariantBaseMap datafields;
-
-        Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
-        datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
-        datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
-        datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
-
-        VirtualRobot::MathTools::Quaternion qtarget = VirtualRobot::MathTools::eigen4f2quat(targetPoseDebug);
-        datafields["target_qw"] = new Variant(qtarget.w);
-        datafields["target_qx"] = new Variant(qtarget.x);
-        datafields["target_qy"] = new Variant(qtarget.y);
-        datafields["target_qz"] = new Variant(qtarget.z);
-
-
-        Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
-        datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
-        datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
-        datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
-
-
-        VirtualRobot::MathTools::Quaternion qcurrent = VirtualRobot::MathTools::eigen4f2quat(currentPoseDebug);
-        datafields["current_qw"] = new Variant(qcurrent.w);
-        datafields["current_qx"] = new Variant(qcurrent.x);
-        datafields["current_qy"] = new Variant(qcurrent.y);
-        datafields["current_qz"] = new Variant(qcurrent.z);
-
-
-        Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
-        datafields["filteredforceInTool_x"] = new Variant(filteredForce(0));
-        datafields["filteredforceInTool_y"] = new Variant(filteredForce(1));
-        datafields["filteredforceInTool_z"] = new Variant(filteredForce(2));
-
-        Eigen::Vector3f filteredTorque = debugRT.getUpToDateReadBuffer().filteredTorque;
-        datafields["filteredtorqueInTool_x"] = new Variant(filteredTorque(0));
-        datafields["filteredtorqueInTool_y"] = new Variant(filteredTorque(1));
-        datafields["filteredtorqueInTool_z"] = new Variant(filteredTorque(2));
-
-
-        Eigen::Vector3f filteredForceInRoot = debugRT.getUpToDateReadBuffer().filteredForceInRoot;
-        datafields["filteredForceInRoot_x"] = new Variant(filteredForceInRoot(0));
-        datafields["filteredForceInRoot_y"] = new Variant(filteredForceInRoot(1));
-        datafields["filteredForceInRoot_z"] = new Variant(filteredForceInRoot(2));
-
-        Eigen::Vector3f rotationAxis = debugRT.getUpToDateReadBuffer().rotationAxis;
-        datafields["rotationAxis_x"] = new Variant(rotationAxis(0));
-        datafields["rotationAxis_y"] = new Variant(rotationAxis(1));
-        datafields["rotationAxis_z"] = new Variant(rotationAxis(2));
-
-        Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
-        datafields["reactForce_x"] = new Variant(reactForce(0));
-        datafields["reactForce_y"] = new Variant(reactForce(1));
-        datafields["reactForce_z"] = new Variant(reactForce(2));
-
-        Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
-        datafields["targetVel_x"] = new Variant(targetVel(0));
-        datafields["targetVel_y"] = new Variant(targetVel(1));
-        datafields["targetVel_z"] = new Variant(targetVel(2));
-        datafields["targetVel_ro"] = new Variant(targetVel(3));
-        datafields["targetVel_pi"] = new Variant(targetVel(4));
-        datafields["targetVel_ya"] = new Variant(targetVel(5));
-
-        Eigen::VectorXf currentTwist = debugRT.getUpToDateReadBuffer().currentTwist;
-        datafields["currentTwist_x"] = new Variant(currentTwist(0));
-        datafields["currentTwist_y"] = new Variant(currentTwist(1));
-        datafields["currentTwist_z"] = new Variant(currentTwist(2));
-        datafields["currentTwist_ro"] = new Variant(currentTwist(3));
-        datafields["currentTwist_pi"] = new Variant(currentTwist(4));
-        datafields["currentTwist_ya"] = new Variant(currentTwist(5));
-
-
-        Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
-        datafields["adaptK_x"] = new Variant(adaptK(0));
-        datafields["adaptK_y"] = new Variant(adaptK(1));
-        datafields["adaptK_z"] = new Variant(adaptK(2));
-
-        Eigen::Vector3f adaptD = debugRT.getUpToDateReadBuffer().adaptD;
-        datafields["adaptD_x"] = new Variant(adaptD(0));
-        datafields["adaptD_y"] = new Variant(adaptD(1));
-        datafields["adaptD_z"] = new Variant(adaptD(2));
-
-        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-        datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
-        datafields["rotAngle"] = new Variant(debugRT.getUpToDateReadBuffer().rotAngle);
-        datafields["wipingCounter"] = new Variant(debugRT.getUpToDateReadBuffer().wipingCounter);
-        datafields["mu"] = new Variant(debugRT.getUpToDateReadBuffer().mu);
-        datafields["loseContactForceIntegral"] = new Variant(debugRT.getUpToDateReadBuffer().loseContactForceIntegral);
-
-        Eigen::VectorXf estFri = debugRT.getUpToDateReadBuffer().estimatedFriction;
-        datafields["estimatedFriction_x"] = new Variant(estFri(0));
-        datafields["estimatedFriction_y"] = new Variant(estFri(1));
-
-        Eigen::VectorXf frictionInToolXY = debugRT.getUpToDateReadBuffer().frictionInToolXY;
-        datafields["frictionInToolXY_x"] = new Variant(frictionInToolXY(0));
-        datafields["frictionInToolXY_y"] = new Variant(frictionInToolXY(1));
-
-        Eigen::VectorXf velPInTool = debugRT.getUpToDateReadBuffer().velPInTool;
-        datafields["velPInTool_x"] = new Variant(velPInTool(0));
-        datafields["velPInTool_y"] = new Variant(velPInTool(1));
-        datafields["velPInTool_z"] = new Variant(velPInTool(2));
-
-        datafields["kp_force_pid"] = new Variant(debugRT.getUpToDateReadBuffer().kpForcePID);
-        datafields["kp_rot_pid"] = new Variant(debugRT.getUpToDateReadBuffer().kpRotPID);
-
-        //        datafields["targetVel_rx"] = new Variant(targetVel(3));
-        //        datafields["targetVel_ry"] = new Variant(targetVel(4));
-        //        datafields["targetVel_rz"] = new Variant(targetVel(5));
-
-        //        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        //        for (auto& pair : values)
-        //        {
-        //            datafieldName = pair.first  + "_" + debugName;
-        //            datafields[datafieldName] = new Variant(pair.second);
-        //        }
-
-        //        auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
-        //        for (auto& pair : currentPose)
-        //        {
-        //            datafieldName = pair.first + "_" + debugName;
-        //            datafields[datafieldName] = new Variant(pair.second);
-        //        }
-
-        //        datafieldName = "canVal_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        //        datafieldName = "mpcFactor_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
-        //        datafieldName = "error_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
-        //        datafieldName = "posError_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
-        //        datafieldName = "oriError_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
-        //        datafieldName = "deltaT_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-        datafieldName = "PeriodicDMP";
-        debugObs->setDebugChannel(datafieldName, datafields);
-
-
-        // draw force;
-        Eigen::Matrix4f globalPose = debugRT.getUpToDateReadBuffer().globalPose;
-        Eigen::Vector3f handPosition = globalPose.block<3, 1>(0, 3);
-        Eigen::Vector3f forceDir = debugRT.getUpToDateReadBuffer().globalFilteredForce;
-
-        debugDrawer->setArrowVisu("Force", "currentForce", new Vector3(handPosition), new Vector3(forceDir), DrawColor {0, 0, 1, 1}, 10 * forceDir.norm(), 3);
-
-        // draw direction of the tool
-        Eigen::Vector3f currentToolDir = debugRT.getUpToDateReadBuffer().currentToolDir;
-        debugDrawer->setArrowVisu("Tool", "Tool", new Vector3(handPosition), new Vector3(currentToolDir), DrawColor {1, 0, 0, 1}, 100, 3);
-        debugDrawer->setPoseVisu("target", "targetPose", new Pose(globalPose));
-
-    }
-
-
-
-    void NJointAnomalyDetectionAdaptiveWipingController::pauseDMP(const Ice::Current&)
-    {
-        dmpCtrl->pauseController();
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::resumeDMP(const Ice::Current&)
-    {
-        dmpCtrl->resumeController();
-    }
-
-    void NJointAnomalyDetectionAdaptiveWipingController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
deleted file mode 100644
index 5a34c6833a7b42ebeae47248cc145cf571093f0c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
+++ /dev/null
@@ -1,277 +0,0 @@
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-namespace armarx
-{
-
-    TYPEDEF_PTRS_HANDLE(NJointAnomalyDetectionAdaptiveWipingController);
-    TYPEDEF_PTRS_HANDLE(NJointAnomalyDetectionAdaptiveWipingControllerControlData);
-
-    class NJointAnomalyDetectionAdaptiveWipingControllerControlData
-    {
-    public:
-        Eigen::VectorXf targetTSVel;
-        Eigen::Matrix4f targetTSPose;
-        double canVal;
-    };
-
-    /**
-     * @brief The NJointAnomalyDetectionAdaptiveWipingController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointAnomalyDetectionAdaptiveWipingController :
-        public NJointControllerWithTripleBuffer<NJointAnomalyDetectionAdaptiveWipingControllerControlData>,
-        public NJointAnomalyDetectionAdaptiveWipingControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr;
-        NJointAnomalyDetectionAdaptiveWipingController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointAnomalyDetectionAdaptiveWipingControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
-        void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
-        bool isFinished(const Ice::Current&)
-        {
-            return false;
-        }
-
-        void setSpeed(Ice::Double times, const Ice::Current&);
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void setAmplitude(Ice::Double amp, const Ice::Current&);
-        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
-        void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
-        double getCanVal(const Ice::Current&)
-        {
-            return dmpCtrl->canVal;
-        }
-        void setTrigerAbnormalEvent(bool abnormal, const Ice::Current&);
-
-        std::vector<float> getAnomalyInput(const Ice::Current&);
-        std::vector<float> getAnomalyOutput(const Ice::Current&);
-
-        void pauseDMP(const Ice::Current&);
-        void resumeDMP(const Ice::Current&);
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitNJointController();
-        void onDisconnectNJointController();
-        void controllerRun();
-
-    private:
-        struct DebugBufferData
-        {
-            StringFloatDictionary latestTargetVelocities;
-            StringFloatDictionary currentPose;
-            double currentCanVal;
-            double mpcFactor;
-            double error;
-            double phaseStop;
-            double posError;
-            double oriError;
-            double deltaT;
-        };
-
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-
-        struct DebugRTData
-        {
-            Eigen::Matrix4f targetPose;
-            Eigen::Vector3f filteredForce;
-            Eigen::Vector3f filteredForceInRoot;
-            Eigen::Vector3f filteredTorque;
-
-            Eigen::Vector3f rotationAxis;
-
-            Eigen::Vector3f reactForce;
-            Eigen::Vector3f adaptK;
-            Eigen::Vector3f adaptD;
-            Eigen::VectorXf targetVel;
-            Eigen::Matrix4f currentPose;
-            bool isPhaseStop;
-
-            Eigen::Matrix4f globalPose;
-            Eigen::Vector3f globalFilteredForce;
-            Eigen::Vector3f currentToolDir;
-            Eigen::VectorXf currentTwist;
-
-            float rotAngle;
-
-            int wipingCounter;
-            float mu;
-            Eigen::Vector2f estimatedFriction;
-            Eigen::Vector3f velPInTool;
-            Eigen::Vector2f frictionInToolXY;
-
-            float kpForcePID;
-            float kpRotPID;
-            float loseContactForceIntegral;
-        };
-        TripleBuffer<DebugRTData> debugRT;
-
-
-        struct RTToControllerData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-            bool isPhaseStop;
-        };
-        TripleBuffer<RTToControllerData> rt2CtrlData;
-
-        struct RTToUserData
-        {
-            Eigen::Matrix4f currentTcpPose;
-            Eigen::Vector3f tcpTranslVel;
-            Eigen::Vector3f forceOutput;
-            float waitTimeForCalibration;
-        };
-        TripleBuffer<RTToUserData> rt2UserData;
-
-        struct UserToRTData
-        {
-            float targetForce;
-        };
-        TripleBuffer<UserToRTData> user2rtData;
-
-
-        TaskSpaceDMPControllerPtr dmpCtrl;
-
-        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-        std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
-        // anomaly detection
-        std::deque<Eigen::VectorXf> velocityHorizonList;
-        size_t velocityHorizon;
-        bool lastAbnormalFlag = false;
-        bool abnormalFlag;
-        bool startLoseContactDetection = false;
-        float forceIntegral;
-        int loseContactCounter = 0;
-        bool isLoseContact = false;
-
-        // velocity ik controller parameters
-        std::string nodeSetName;
-
-        bool started;
-        bool firstRun;
-        bool dmpRunning;
-
-        // friction estimation
-        float mu = 1.5f; // init friction coefficient
-        Eigen::Vector2f lastForceInToolXY;
-        double lastCanVal = 0.0;
-        int wipingCounter = 0;
-        std::deque<float> recordFrictionNorm;
-        std::deque<float> recordForceNormToSurface;
-        size_t frictionHorizon;
-        Eigen::Vector2f estimatedFriction;
-        float safeFrictionConeLowerLimit = 0.2;
-
-        // lose contact detection
-        float loseContactRatio = 0.2f;
-        int makingContactCounter = 0;
-        bool isMakingContact = false;
-        bool isLCREnabled = false;
-        bool isContactedOnce = false;
-        float forceControlGate = 1.0;
-        int lcrCounter = 500;
-
-        VirtualRobot::DifferentialIKPtr ik;
-        VirtualRobot::RobotNodePtr tcp;
-
-        NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr cfg;
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointAnomalyDetectionAdaptiveWipingController>::pointer_type controllerTask;
-        Eigen::Matrix4f targetPose;
-        Eigen::Matrix4f initHandPose;
-
-        Eigen::Vector3f kpos;
-        Eigen::Vector3f dpos;
-        Eigen::Vector3f kori;
-        Eigen::Vector3f dori;
-        Eigen::VectorXf knull;
-        Eigen::VectorXf dnull;
-
-        Eigen::Vector3f adaptK;
-        Eigen::Vector3f adaptD;
-        Eigen::Vector3f adaptKOri;
-        Eigen::Vector3f adaptDOri;
-        Eigen::VectorXf adaptKNull;
-        Eigen::VectorXf adaptDNull;
-
-        Eigen::VectorXf nullSpaceJointsVec;
-        const SensorValueForceTorque* forceSensor;
-
-        // pid controllers
-        bool isForceCtrlInForceDir;
-        bool isForceControlEnabled;
-        bool isRotControlEnabled;
-        bool isTorqueControlEnabled;
-        bool isLCRControlEnabled;
-        PIDControllerPtr forcePID;
-        PIDControllerPtr rotPID;
-        PIDControllerPtr torquePID;
-        PIDControllerPtr lcrPID; // lose contact recover pid controller
-        float adaptKpForce;
-        float adaptKpRot;
-
-        // force torque related
-        Eigen::Vector6f targetFTInToolFrame;
-        Eigen::Vector3f filteredForce;
-        Eigen::Vector3f filteredTorque;
-        Eigen::Vector6f filteredFTCommand;
-        Eigen::Vector3f forceOffset;
-        Eigen::Vector3f currentForceOffset;
-
-        Eigen::Vector3f torqueOffset;
-        Eigen::Vector3f currentTorqueOffset;
-        float handMass;
-        Eigen::Vector3f handCOM;
-        Eigen::Vector3f gravityInRoot;
-
-        Eigen::Vector3f filteredForceInRoot;
-        Eigen::Vector3f filteredTorqueInRoot;
-
-        Eigen::Matrix3f toolTransform;
-        Eigen::Vector3f oriToolDir;
-        Eigen::Matrix3f origHandOri;
-        Eigen::VectorXf qvel_filtered;
-
-        bool useDMPInGlobalFrame;
-
-        float lastDiff;
-        Eigen::Vector2f lastPosition;
-        double changeTimer;
-
-        Eigen::Vector3f toolToFTSensorLink;
-        Eigen::Vector3f positionOffset;
-
-    };
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
deleted file mode 100644
index d61b8fa3f02fb55eb0311332c289c6df6febcae3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ /dev/null
@@ -1,850 +0,0 @@
-#include "NJointBimanualCCDMPController.h"
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointBimanualCCDMPController> registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController");
-
-    NJointBimanualCCDMPController::NJointBimanualCCDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "Preparing ... ";
-        cfg = NJointBimanualCCDMPControllerConfigPtr::dynamicCast(config);
-
-        ARMARX_CHECK_EXPRESSION(robotUnit);
-        useSynchronizedRtRobot();
-
-        leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-        leftNullSpaceCoefs.resize(leftRNS->getSize());
-        leftNullSpaceCoefs.setOnes();
-
-        for (size_t i = 0; i < leftRNS->getSize(); ++i)
-        {
-            std::string jointName = leftRNS->getNode(i)->getName();
-
-            if (leftRNS->getNode(i)->isLimitless())
-            {
-                leftNullSpaceCoefs(i) = 1;
-            }
-
-            leftJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-            if (!accelerationSensor)
-            {
-                ARMARX_WARNING << "No accelerationSensor available for " << jointName;
-            }
-
-
-            leftVelocitySensors.push_back(velocitySensor);
-            leftPositionSensors.push_back(positionSensor);
-            leftAccelerationSensors.push_back(accelerationSensor);
-
-        };
-        rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
-        rightNullSpaceCoefs.resize(rightRNS->getSize());
-        rightNullSpaceCoefs.setOnes();
-        for (size_t i = 0; i < rightRNS->getSize(); ++i)
-        {
-            std::string jointName = rightRNS->getNode(i)->getName();
-
-            if (rightRNS->getNode(i)->isLimitless())
-            {
-                rightNullSpaceCoefs(i) = 1;
-            }
-
-            rightJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-            if (!accelerationSensor)
-            {
-                ARMARX_WARNING << "No accelerationSensor available for " << jointName;
-            }
-
-            rightVelocitySensors.push_back(velocitySensor);
-            rightPositionSensors.push_back(positionSensor);
-            rightAccelerationSensors.push_back(accelerationSensor);
-
-        };
-
-
-        const SensorValueBase* svlf = useSensorValue("FT L");
-        leftForceTorque = svlf->asA<SensorValueForceTorque>();
-        const SensorValueBase* svrf = useSensorValue("FT R");
-        rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
-        leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
-        taskSpaceDMPConfig.DMPAmplitude = 1.0;
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-
-        TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig, false));
-        TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig, false));
-        TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig, false));
-        TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig, false));
-
-        leftGroup.push_back(leftLeader);
-        leftGroup.push_back(rightFollower);
-
-        rightGroup.push_back(rightLeader);
-        rightGroup.push_back(leftFollower);
-
-        bothLeaderGroup.push_back(leftLeader);
-        bothLeaderGroup.push_back(rightLeader);
-
-
-        tcpLeft = leftRNS->getTCP();
-        tcpRight = rightRNS->getTCP();
-
-
-        leaderName = cfg->defautLeader;
-
-
-        leftDesiredJointValues.resize(leftTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
-        }
-
-        rightDesiredJointValues.resize(rightTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
-        }
-
-
-        leftKpos << cfg->leftKpos[0], cfg->leftKpos[1], cfg->leftKpos[2];
-        leftDpos << cfg->leftDpos[0], cfg->leftDpos[1], cfg->leftDpos[2];
-        leftKori << cfg->leftKori[0], cfg->leftKori[1], cfg->leftKori[2];
-        leftDori << cfg->leftDori[0], cfg->leftDori[1], cfg->leftDori[2];
-
-        rightKpos << cfg->rightKpos[0], cfg->rightKpos[1], cfg->rightKpos[2];
-        rightDpos << cfg->rightDpos[0], cfg->rightDpos[1], cfg->rightDpos[2];
-        rightKori << cfg->rightKori[0], cfg->rightKori[1], cfg->rightKori[2];
-        rightDori << cfg->rightDori[0], cfg->rightDori[1], cfg->rightDori[2];
-
-        knull = cfg->knull;
-        dnull = cfg->dnull;
-
-
-        torqueLimit = cfg->torqueLimit;
-
-
-        maxLinearVel = cfg->maxLinearVel;
-        maxAngularVel = cfg->maxAngularVel;
-
-        torqueFactor = 1.0;
-        startReduceTorque = cfg->startReduceTorque;
-
-        NJointBimanualCCDMPControllerInterfaceData initInterfaceData;
-        initInterfaceData.currentLeftPose = Eigen::Matrix4f::Identity();
-        initInterfaceData.currentRightPose = Eigen::Matrix4f::Identity();
-        interfaceData.reinitAllBuffers(initInterfaceData);
-
-        NJointBimanualCCDMPControllerSensorData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentLeftPose = Eigen::Matrix4f::Identity();
-        initSensorData.currentRightPose = Eigen::Matrix4f::Identity();
-        controllerSensorData.reinitAllBuffers(initSensorData);
-
-    }
-
-    std::string NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointBimanualCCDMPController";
-    }
-
-    void NJointBimanualCCDMPController::rtPreActivateController()
-    {
-        NJointBimanualCCDMPControllerControlData initData;
-        initData.leftTargetVel.resize(6);
-        initData.leftTargetVel.setZero();
-        initData.rightTargetVel.resize(6);
-        initData.rightTargetVel.setZero();
-        initData.leftTargetPose = tcpLeft->getPoseInRootFrame();
-        initData.rightTargetPose = tcpRight->getPoseInRootFrame();
-        initData.virtualTime = cfg->timeDuration;
-        reinitTripleBuffer(initData);
-    }
-
-
-    void NJointBimanualCCDMPController::controllerRun()
-    {
-        if (!controllerSensorData.updateReadBuffer())
-        {
-            return;
-        }
-
-        double deltaT = controllerSensorData.getReadBuffer().deltaT;
-
-
-        Eigen::VectorXf leftTargetVel;
-        leftTargetVel.resize(6);
-        leftTargetVel.setZero();
-        Eigen::VectorXf rightTargetVel;
-        rightTargetVel.resize(6);
-        rightTargetVel.setZero();
-
-        std::vector<TaskSpaceDMPControllerPtr > currentControlGroup;
-        Eigen::Matrix4f currentLeaderPose;
-        Eigen::Matrix4f currentFollowerPose;
-        Eigen::VectorXf currentLeaderTwist;
-        Eigen::VectorXf currentFollowerTwist;
-        if (leaderName == "Left")
-        {
-            currentControlGroup = leftGroup;
-            currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
-            currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
-            currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
-            currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
-        }
-        else if (leaderName == "right")
-        {
-            currentControlGroup = rightGroup;
-            currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
-            currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
-            currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
-            currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
-        }
-        else
-        {
-            currentControlGroup = bothLeaderGroup;
-
-            TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
-            TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
-            leaderDMPleft->flow(deltaT, controllerSensorData.getReadBuffer().currentLeftPose, controllerSensorData.getReadBuffer().currentLeftTwist);
-            leaderDMPright->flow(deltaT, controllerSensorData.getReadBuffer().currentRightPose, controllerSensorData.getReadBuffer().currentRightTwist);
-
-            Eigen::VectorXf leftTargetVel = leaderDMPleft->getTargetVelocity();
-            Eigen::Matrix4f leftTargetPose = leaderDMPleft->getTargetPoseMat();
-            Eigen::VectorXf rightTargetVel = leaderDMPright->getTargetVelocity();
-            Eigen::Matrix4f rightTargetPose = leaderDMPright->getTargetPoseMat();
-
-            virtualtimer = leaderDMPleft->canVal;
-            LockGuardType guard {controlDataMutex};
-            getWriterControlStruct().leftTargetVel = leftTargetVel;
-            getWriterControlStruct().rightTargetVel = rightTargetVel;
-            getWriterControlStruct().leftTargetPose = leftTargetPose;
-            getWriterControlStruct().rightTargetPose = rightTargetPose;
-            writeControlStruct();
-
-            return;
-        }
-
-        TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
-        TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
-        virtualtimer = leaderDMP->canVal;
-
-        if (virtualtimer < 1e-8)
-        {
-            finished = true;
-        }
-
-
-        leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
-
-        Eigen::Matrix4f currentFollowerLocalPose;
-        currentFollowerLocalPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0).inverse() * currentFollowerPose.block<3, 3>(0, 0);
-        currentFollowerLocalPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0).inverse() * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
-        followerDMP->flow(deltaT, currentFollowerLocalPose, currentFollowerTwist);
-
-        Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
-        Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
-        Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
-        std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
-
-        Eigen::Matrix4f followerTargetPose;
-        followerTargetPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 3>(0, 0);
-        followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
-
-
-        Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
-        Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
-        followerTargetVel.setZero();
-
-        //        followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
-        //        followerTargetVel.block<3, 1>(0, 0) = KposFollower * (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
-        //        Eigen::Matrix3f followerDiffMat =  followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
-        //        Eigen::Vector3f followerDiffRPY = KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
-        //        followerTargetVel(3) = followerDiffRPY(0);
-        //        followerTargetVel(4) = followerDiffRPY(1);
-        //        followerTargetVel(5) = followerDiffRPY(2);
-
-
-
-
-
-        std::vector<double> leftDMPTarget;
-        std::vector<double> rightDMPTarget;
-
-        Eigen::Matrix4f leftTargetPose;
-        Eigen::Matrix4f rightTargetPose;
-
-        //        float leftKratio = 1.0;
-        //        float rightKratio = 1.0;
-
-        if (leaderName == "Left")
-        {
-            leftTargetVel = leaderTargetVel;
-            rightTargetVel = followerTargetVel;
-
-            leftDMPTarget = leaderDMP->getTargetPose();
-            rightDMPTarget = followerLocalTargetPoseVec;
-
-
-            leftTargetPose = leaderTargetPose;
-            rightTargetPose = followerTargetPose;
-
-
-        }
-        else if (leaderName == "right")
-        {
-            rightTargetVel = leaderTargetVel;
-            leftTargetVel = followerTargetVel;
-
-            rightDMPTarget = leaderDMP->getTargetPose();
-            leftDMPTarget = followerLocalTargetPoseVec;
-
-            rightTargetPose = leaderTargetPose;
-            leftTargetPose = followerTargetPose;
-
-        }
-
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().leftTargetVel = leftTargetVel;
-        getWriterControlStruct().rightTargetVel = rightTargetVel;
-        getWriterControlStruct().leftTargetPose = leftTargetPose;
-        getWriterControlStruct().rightTargetPose = rightTargetPose;
-        getWriterControlStruct().virtualTime = virtualtimer;
-
-        writeControlStruct();
-    }
-
-    Eigen::VectorXf NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf& tcptwist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose)
-    {
-        //        Eigen::Vector3f currentTCPLinearVelocity;
-        //        currentTCPLinearVelocity << 0.001 * tcptwist(0),   0.001 * tcptwist(1), 0.001 * tcptwist(2);
-        //        Eigen::Vector3f currentTCPAngularVelocity;
-        //        currentTCPAngularVelocity << tcptwist(3),   tcptwist(4),  tcptwist(5);
-
-        //        Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
-        //        Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
-        //        Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
-        //        Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
-
-        //        Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
-        //        Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
-        //        Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        //        Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
-        //        Eigen::Vector6f tcpDesiredWrench;
-        //        tcpDesiredWrench <<   0.001 * tcpDesiredForce, tcpDesiredTorque;
-
-        //        return tcpDesiredWrench;
-        return Eigen::Vector6f::Zero();
-    }
-
-
-
-    void NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-
-
-        interfaceData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
-        interfaceData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
-        interfaceData.commitWrite();
-
-        controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
-        controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
-        controllerSensorData.getWriteBuffer().deltaT = deltaT;
-        controllerSensorData.getWriteBuffer().currentTime += deltaT;
-
-        // cartesian vel controller
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-
-        Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf leftqpos;
-        Eigen::VectorXf leftqvel;
-        leftqpos.resize(leftPositionSensors.size());
-        leftqvel.resize(leftVelocitySensors.size());
-        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
-        {
-            leftqpos(i) = leftPositionSensors[i]->position;
-            leftqvel(i) = leftVelocitySensors[i]->velocity;
-        }
-
-        Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-
-
-        Eigen::VectorXf rightqpos;
-        Eigen::VectorXf rightqvel;
-        rightqpos.resize(rightPositionSensors.size());
-        rightqvel.resize(rightVelocitySensors.size());
-
-        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
-        {
-            rightqpos(i) = rightPositionSensors[i]->position;
-            rightqvel(i) = rightVelocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
-        Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
-        controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
-        controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
-        controllerSensorData.commitWrite();
-
-
-        jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
-        jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
-
-        Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
-        Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
-        double virtualtime = rtGetControlStruct().virtualTime;
-        Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
-
-        Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
-        float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
-        if (normLinearVelocity > maxLinearVel)
-        {
-            leftTargetVel.block<3, 1>(0, 0) = maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
-        }
-        float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
-        if (normAngularVelocity > maxAngularVel)
-        {
-            leftTargetVel.block<3, 1>(3, 0) = maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
-        }
-
-        Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
-        normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
-        if (normLinearVelocity > maxLinearVel)
-        {
-            rightTargetVel.block<3, 1>(0, 0) = maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
-        }
-        normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
-        if (normAngularVelocity > maxAngularVel)
-        {
-            rightTargetVel.block<3, 1>(3, 0) = maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
-        }
-
-
-
-        // unconstrained space controller
-        Eigen::Vector6f leftJointControlWrench;
-        {
-
-
-            Eigen::Vector3f targetTCPLinearVelocity;
-            targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1), 0.001 * leftTargetVel(2);
-
-            Eigen::Vector3f currentTCPLinearVelocity;
-            currentTCPLinearVelocity <<  0.001 * currentLeftTwist(0),  0.001 * currentLeftTwist(1),   0.001 * currentLeftTwist(2);
-            Eigen::Vector3f currentTCPAngularVelocity;
-            currentTCPAngularVelocity << currentLeftTwist(3),   currentLeftTwist(4),  currentLeftTwist(5);
-            Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
-            Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
-
-            Eigen::Vector3f tcpDesiredForce = 0.001 * leftKpos.cwiseProduct(desiredPosition - currentTCPPosition) + leftDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-            Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
-            Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-            Eigen::Vector3f tcpDesiredTorque = leftKori.cwiseProduct(rpy) - leftDori.cwiseProduct(currentTCPAngularVelocity);
-            leftJointControlWrench <<  tcpDesiredForce, tcpDesiredTorque;
-        }
-
-        Eigen::Vector6f rightJointControlWrench;
-        {
-
-            Eigen::Vector3f targetTCPLinearVelocity;
-            targetTCPLinearVelocity << 0.001 * rightTargetVel(0), 0.001 * rightTargetVel(1), 0.001 * rightTargetVel(2);
-
-            Eigen::Vector3f currentTCPLinearVelocity;
-            currentTCPLinearVelocity <<   0.001 * currentRightTwist(0),   0.001 * currentRightTwist(1),  0.001 * currentRightTwist(2);
-            Eigen::Vector3f currentTCPAngularVelocity;
-            currentTCPAngularVelocity << currentRightTwist(3),   currentRightTwist(4),  currentRightTwist(5);
-            Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
-            Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
-
-            Eigen::Vector3f tcpDesiredForce = 0.001 * rightKpos.cwiseProduct(desiredPosition - currentTCPPosition) + rightDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-            Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
-            Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-            Eigen::Vector3f tcpDesiredTorque = rightKori.cwiseProduct(rpy) - rightDori.cwiseProduct(currentTCPAngularVelocity);
-            rightJointControlWrench <<   tcpDesiredForce, tcpDesiredTorque;
-        }
-
-
-
-        //        Eigen::VectorXf leftJointLimitAvoidance(leftRNS->getSize());
-        //        for (size_t i = 0; i < leftRNS->getSize(); i++)
-        //        {
-        //            VirtualRobot::RobotNodePtr rn = leftRNS->getNode(i);
-        //            if (rn->isLimitless())
-        //            {
-        //                leftJointLimitAvoidance(i) = 0;
-        //            }
-        //            else
-        //            {
-        //                float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
-        //                leftJointLimitAvoidance(i) = cos(f * M_PI);
-        //            }
-        //        }
-        //        Eigen::VectorXf leftNullspaceTorque = knull * leftJointLimitAvoidance - dnull * leftqvel;
-        //        Eigen::VectorXf rightJointLimitAvoidance(rightRNS->getSize());
-        //        for (size_t i = 0; i < rightRNS->getSize(); i++)
-        //        {
-        //            VirtualRobot::RobotNodePtr rn = rightRNS->getNode(i);
-        //            if (rn->isLimitless())
-        //            {
-        //                rightJointLimitAvoidance(i) = 0;
-        //            }
-        //            else
-        //            {
-        //                float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
-        //                rightJointLimitAvoidance(i) = cos(f * M_PI);
-        //            }
-        //        }
-        //        Eigen::VectorXf rightNullspaceTorque = knull * rightJointLimitAvoidance - dnull * rightqvel;
-
-
-        Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
-        Eigen::VectorXf rightNullspaceTorque = knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
-
-        float lambda = 2;
-
-        Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
-        Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
-        Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullSpaceCoefs.cwiseProduct(leftNullspaceTorque);
-        Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullSpaceCoefs.cwiseProduct(rightNullspaceTorque);
-
-
-
-        // torque limit
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            float desiredTorque =   leftJointDesiredTorques(i);
-
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-
-            desiredTorque = (desiredTorque >  torqueLimit) ? torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
-            debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = desiredTorque;
-
-            leftTargets.at(i)->torque = desiredTorque;
-        }
-
-
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            float desiredTorque = rightJointDesiredTorques(i);
-
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-
-            desiredTorque = (desiredTorque >  torqueLimit) ? torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
-            debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = desiredTorque;
-
-            rightTargets.at(i)->torque = desiredTorque;
-        }
-        debugDataInfo.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
-
-        debugDataInfo.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
-        debugDataInfo.getWriteBuffer().leftControlSignal_y = leftJointControlWrench(1);
-        debugDataInfo.getWriteBuffer().leftControlSignal_z = leftJointControlWrench(2);
-        debugDataInfo.getWriteBuffer().leftControlSignal_ro = leftJointControlWrench(3);
-        debugDataInfo.getWriteBuffer().leftControlSignal_pi = leftJointControlWrench(4);
-        debugDataInfo.getWriteBuffer().leftControlSignal_ya = leftJointControlWrench(5);
-
-        debugDataInfo.getWriteBuffer().rightControlSignal_x = rightJointControlWrench(0);
-        debugDataInfo.getWriteBuffer().rightControlSignal_y = rightJointControlWrench(1);
-        debugDataInfo.getWriteBuffer().rightControlSignal_z = rightJointControlWrench(2);
-        debugDataInfo.getWriteBuffer().rightControlSignal_ro = rightJointControlWrench(3);
-        debugDataInfo.getWriteBuffer().rightControlSignal_pi = rightJointControlWrench(4);
-        debugDataInfo.getWriteBuffer().rightControlSignal_ya = rightJointControlWrench(5);
-        //        debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
-        //        debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
-        //        debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
-        //        debugDataInfo.getWriteBuffer().quatError = errorAngle;
-        //        debugDataInfo.getWriteBuffer().posiError = posiError;
-        debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
-        debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
-        debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
-        debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
-        debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
-        debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
-
-
-        debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
-        debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
-        debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
-
-        debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
-        debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
-        debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
-        debugDataInfo.getWriteBuffer().virtualTime = virtualtime;
-
-        debugDataInfo.commitWrite();
-
-
-
-
-        //        Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
-        //        Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
-        //        Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
-
-        //        for (size_t i = 0; i < leftTargets.size(); ++i)
-        //        {
-        //            leftTargets.at(i)->velocity = jnvL(i);
-        //        }
-        //        Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
-        //        Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
-        //        Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
-
-        //        for (size_t i = 0; i < rightTargets.size(); ++i)
-        //        {
-        //            rightTargets.at(i)->velocity = jnvR(i);
-        //        }
-    }
-
-    void NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        if (name == "LeftLeader")
-        {
-            leftGroup.at(0)->learnDMPFromFiles(fileNames);
-        }
-        else if (name == "LeftFollower")
-        {
-            rightGroup.at(1)->learnDMPFromFiles(fileNames);
-        }
-        else if (name == "RightLeader")
-        {
-            rightGroup.at(0)->learnDMPFromFiles(fileNames);
-        }
-        else
-        {
-            leftGroup.at(1)->learnDMPFromFiles(fileNames);
-        }
-    }
-
-
-    void NJointBimanualCCDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-        if (leaderName == "Left")
-        {
-            leftGroup.at(0)->setViaPose(u, viapoint);
-        }
-        else
-        {
-            rightGroup.at(0)->setViaPose(u, viapoint);
-        }
-    }
-
-    void NJointBimanualCCDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard(controllerMutex);
-        if (leaderName == "Left")
-        {
-            leftGroup.at(0)->setGoalPoseVec(goals);
-        }
-        else
-        {
-            rightGroup.at(0)->setGoalPoseVec(goals);
-        }
-
-    }
-
-    void NJointBimanualCCDMPController::changeLeader(const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-
-        if (leaderName == "Left")
-        {
-            leaderName = "Right";
-            rightGroup.at(0)->canVal = virtualtimer;
-            rightGroup.at(1)->canVal = virtualtimer;
-        }
-        else
-        {
-            leaderName = "Left";
-            leftGroup.at(0)->canVal = virtualtimer;
-            leftGroup.at(1)->canVal = virtualtimer;
-        }
-
-    }
-
-
-
-
-
-    void NJointBimanualCCDMPController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
-    {
-
-        while (!interfaceData.updateReadBuffer())
-        {
-            usleep(100);
-        }
-        Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().currentLeftPose;
-        Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().currentRightPose;
-
-        ARMARX_IMPORTANT << VAROUT(leftPose);
-        ARMARX_IMPORTANT << VAROUT(rightPose);
-
-        virtualtimer = cfg->timeDuration;
-
-        leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
-        rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
-
-
-        ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
-
-        leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
-        rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
-
-        finished = false;
-        controllerTask->start();
-    }
-
-    Eigen::Matrix4f NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
-    {
-        Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
-
-        localPose.block<3, 3>(0, 0) = newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
-        localPose.block<3, 1>(0, 3) = newCoordinate.block<3, 3>(0, 0).inverse() * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
-
-
-        return localPose;
-    }
-
-
-
-    void NJointBimanualCCDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-
-        StringVariantBaseMap datafields;
-        auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
-        for (auto& pair : constrained_force)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-
-        datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
-        datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
-        datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
-        datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
-        datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
-        datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
-
-
-        datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
-        datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
-        datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
-        datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
-        datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
-        datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
-
-        datafields["leftControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_x);
-        datafields["leftControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_y);
-        datafields["leftControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_z);
-        datafields["leftControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ro);
-        datafields["leftControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_pi);
-        datafields["leftControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ya);
-
-
-        datafields["rightControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_x);
-        datafields["rightControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_y);
-        datafields["rightControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_z);
-        datafields["rightControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ro);
-        datafields["rightControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_pi);
-        datafields["rightControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ya);
-
-        datafields["virtual_timer"] = new Variant(debugDataInfo.getUpToDateReadBuffer().virtualTime);
-
-
-        debugObs->setDebugChannel("DMPController", datafields);
-    }
-
-    void NJointBimanualCCDMPController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        controllerTask = new PeriodicTask<NJointBimanualCCDMPController>(this, &NJointBimanualCCDMPController::controllerRun, 0.3);
-    }
-
-    void NJointBimanualCCDMPController::onDisconnectNJointController()
-    {
-        controllerTask->stop();
-        ARMARX_INFO << "stopped ...";
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
deleted file mode 100644
index 71939269fbcec97ee6a1ebea7b9c8f654a669554..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
+++ /dev/null
@@ -1,263 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-
-namespace armarx
-{
-    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPController);
-    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPControllerControlData);
-
-    using ViaPoint = std::pair<double, DMP::DVec >;
-    using ViaPointsSet = std::vector<ViaPoint >;
-    class NJointBimanualCCDMPControllerControlData
-    {
-    public:
-        Eigen::VectorXf leftTargetVel;
-        Eigen::VectorXf rightTargetVel;
-
-        Eigen::Matrix4f leftTargetPose;
-        Eigen::Matrix4f rightTargetPose;
-
-        double virtualTime;
-
-    };
-
-    /**
-     * @brief The NJointBimanualCCDMPController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointBimanualCCDMPController :
-        public NJointControllerWithTripleBuffer<NJointBimanualCCDMPControllerControlData>,
-        public NJointBimanualCCDMPControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointBimanualCCDMPControllerConfigPtr;
-        NJointBimanualCCDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const override;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        // NJointBimanualCCDMPControllerInterface interface
-        void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&) override;
-        bool isFinished(const Ice::Current&) override
-        {
-            return finished;
-        }
-
-        void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&) override;
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
-        void changeLeader(const Ice::Current&) override;
-
-        double getVirtualTime(const Ice::Current&) override
-        {
-            return virtualtimer;
-        }
-
-        std::string getLeaderName(const Ice::Current&) override
-        {
-            return leaderName;
-        }
-
-    protected:
-
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
-        void onInitNJointController() override;
-        void onDisconnectNJointController() override;
-        void controllerRun();
-    private:
-
-        Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose);
-
-        Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
-        Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
-        {
-            Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
-            newCoordinate(0, 3) = newCoordinateVec.at(0);
-            newCoordinate(1, 3) = newCoordinateVec.at(1);
-            newCoordinate(2, 3) = newCoordinateVec.at(2);
-
-            Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
-            globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
-            globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
-            globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
-
-            return getLocalPose(newCoordinate, globalTargetPose);
-
-        }
-
-        struct DebugBufferData
-        {
-            StringFloatDictionary desired_torques;
-            StringFloatDictionary constrained_force;
-            float leftTargetPose_x;
-            float leftTargetPose_y;
-            float leftTargetPose_z;
-            float rightTargetPose_x;
-            float rightTargetPose_y;
-            float rightTargetPose_z;
-
-            float leftCurrentPose_x;
-            float leftCurrentPose_y;
-            float leftCurrentPose_z;
-            float rightCurrentPose_x;
-            float rightCurrentPose_y;
-            float rightCurrentPose_z;
-
-            float leftControlSignal_x;
-            float leftControlSignal_y;
-            float leftControlSignal_z;
-            float leftControlSignal_ro;
-            float leftControlSignal_pi;
-            float leftControlSignal_ya;
-
-
-            float rightControlSignal_x;
-            float rightControlSignal_y;
-            float rightControlSignal_z;
-            float rightControlSignal_ro;
-            float rightControlSignal_pi;
-            float rightControlSignal_ya;
-
-            //            StringFloatDictionary latestTargetVelocities;
-            //            StringFloatDictionary dmpTargets;
-            //            StringFloatDictionary currentPose;
-
-            //            double leadermpcFactor;
-            //            double leadererror;
-            //            double leaderposError;
-            //            double leaderoriError;
-            //            double leaderCanVal;
-
-            //            double followermpcFactor;
-            //            double followererror;
-            //            double followerposError;
-            //            double followeroriError;
-            //            double followerCanVal;
-
-            double virtualTime;
-
-        };
-
-        bool finished;
-        TripleBuffer<DebugBufferData> debugDataInfo;
-
-        struct NJointBimanualCCDMPControllerSensorData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentLeftPose;
-            Eigen::Matrix4f currentRightPose;
-            Eigen::VectorXf currentLeftTwist;
-            Eigen::VectorXf currentRightTwist;
-
-        };
-        TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
-
-        struct NJointBimanualCCDMPControllerInterfaceData
-        {
-            Eigen::Matrix4f currentLeftPose;
-            Eigen::Matrix4f currentRightPose;
-        };
-
-        TripleBuffer<NJointBimanualCCDMPControllerInterfaceData> interfaceData;
-
-
-        std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
-        std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
-        const SensorValueForceTorque* rightForceTorque;
-        const SensorValueForceTorque* leftForceTorque;
-
-        NJointBimanualCCDMPControllerConfigPtr cfg;
-        VirtualRobot::DifferentialIKPtr leftIK;
-        VirtualRobot::DifferentialIKPtr rightIK;
-
-        std::vector<TaskSpaceDMPControllerPtr > leftGroup;
-        std::vector<TaskSpaceDMPControllerPtr > rightGroup;
-        std::vector<TaskSpaceDMPControllerPtr > bothLeaderGroup;
-
-
-        std::string leaderName;
-
-        VirtualRobot::RobotNodePtr tcpLeft;
-        VirtualRobot::RobotNodePtr tcpRight;
-
-        double virtualtimer;
-
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointBimanualCCDMPController>::pointer_type controllerTask;
-
-        Eigen::VectorXf leftDesiredJointValues;
-        Eigen::VectorXf rightDesiredJointValues;
-
-        Eigen::Vector3f leftKpos;
-        Eigen::Vector3f leftKori;
-        Eigen::Vector3f leftDpos;
-        Eigen::Vector3f leftDori;
-
-        Eigen::Vector3f rightKpos;
-        Eigen::Vector3f rightKori;
-        Eigen::Vector3f rightDpos;
-        Eigen::Vector3f rightDori;
-
-
-        float knull;
-        float dnull;
-
-        std::vector<std::string> leftJointNames;
-        std::vector<std::string> rightJointNames;
-
-        float torqueLimit;
-        VirtualRobot::RobotNodeSetPtr leftRNS;
-        VirtualRobot::RobotNodeSetPtr rightRNS;
-
-        Eigen::VectorXf leftNullSpaceCoefs;
-        Eigen::VectorXf rightNullSpaceCoefs;
-
-        float torqueFactor;
-        float startReduceTorque;
-
-        float maxLinearVel;
-        float maxAngularVel;
-
-
-
-        // NJointBimanualCCDMPControllerInterface interface
-
-        // NJointController interface
-    protected:
-        void rtPreActivateController() override;
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
deleted file mode 100644
index f62e71798edaaac3e00ddd5e9b243f362415cbd6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
+++ /dev/null
@@ -1,855 +0,0 @@
-#include "NJointBimanualCCDMPVelocityController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointBimanualCCDMPVelocityController> registrationControllerNJointBimanualCCDMPVelocityController("NJointBimanualCCDMPVelocityController");
-
-    NJointBimanualCCDMPVelocityController::NJointBimanualCCDMPVelocityController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "Preparing ... ";
-        cfg = NJointBimanualCCDMPVelocityControllerConfigPtr::dynamicCast(config);
-
-        ARMARX_CHECK_EXPRESSION(robotUnit);
-        useSynchronizedRtRobot();
-
-        leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-        leftNullSpaceCoefs.resize(leftRNS->getSize());
-        leftNullSpaceCoefs.setOnes();
-
-        for (size_t i = 0; i < leftRNS->getSize(); ++i)
-        {
-            std::string jointName = leftRNS->getNode(i)->getName();
-
-            if (leftRNS->getNode(i)->isLimitless())
-            {
-                leftNullSpaceCoefs(i) = 1;
-            }
-
-            leftJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-            if (!accelerationSensor)
-            {
-                ARMARX_WARNING << "No accelerationSensor available for " << jointName;
-            }
-
-
-            leftVelocitySensors.push_back(velocitySensor);
-            leftPositionSensors.push_back(positionSensor);
-            leftAccelerationSensors.push_back(accelerationSensor);
-
-        };
-        rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
-        rightNullSpaceCoefs.resize(rightRNS->getSize());
-        rightNullSpaceCoefs.setOnes();
-        for (size_t i = 0; i < rightRNS->getSize(); ++i)
-        {
-            std::string jointName = rightRNS->getNode(i)->getName();
-
-            if (rightRNS->getNode(i)->isLimitless())
-            {
-                rightNullSpaceCoefs(i) = 1;
-            }
-
-            rightJointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-            if (!accelerationSensor)
-            {
-                ARMARX_WARNING << "No accelerationSensor available for " << jointName;
-            }
-
-            rightVelocitySensors.push_back(velocitySensor);
-            rightPositionSensors.push_back(positionSensor);
-            rightAccelerationSensors.push_back(accelerationSensor);
-
-        };
-
-
-        const SensorValueBase* svlf = useSensorValue("FT L");
-        leftForceTorque = svlf->asA<SensorValueForceTorque>();
-        const SensorValueBase* svrf = useSensorValue("FT R");
-        rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
-        leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-        leftCtrl.reset(new CartesianVelocityController(leftRNS, leftRNS->getTCP()));
-        rightCtrl.reset(new CartesianVelocityController(rightRNS, rightRNS->getTCP()));
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
-        taskSpaceDMPConfig.DMPAmplitude = 1.0;
-
-
-        TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig, false));
-        TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig, false));
-        TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig, false));
-        TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig, false));
-        leftJointDMP.reset(new DMP::UMIDMP(10));
-        rightJointDMP.reset(new DMP::UMIDMP(10));
-        isLeftJointLearned = false;
-        isRightJointLearned = false;
-
-        started = false;
-        isDMPRun = false;
-
-        leftGroup.push_back(leftLeader);
-        leftGroup.push_back(rightFollower);
-
-        rightGroup.push_back(rightLeader);
-        rightGroup.push_back(leftFollower);
-
-        bothLeaderGroup.push_back(leftLeader);
-        bothLeaderGroup.push_back(rightLeader);
-
-
-        tcpLeft = leftRNS->getTCP();
-        tcpRight = rightRNS->getTCP();
-
-
-        leaderName = cfg->defautLeader;
-
-
-        leftDesiredJointValues.resize(leftTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
-        }
-
-        rightDesiredJointValues.resize(rightTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
-        }
-
-
-        leftKpos << cfg->leftKpos[0], cfg->leftKpos[1], cfg->leftKpos[2];
-        leftDpos << cfg->leftDpos[0], cfg->leftDpos[1], cfg->leftDpos[2];
-        leftKori << cfg->leftKori[0], cfg->leftKori[1], cfg->leftKori[2];
-        leftDori << cfg->leftDori[0], cfg->leftDori[1], cfg->leftDori[2];
-
-        rightKpos << cfg->rightKpos[0], cfg->rightKpos[1], cfg->rightKpos[2];
-        rightDpos << cfg->rightDpos[0], cfg->rightDpos[1], cfg->rightDpos[2];
-        rightKori << cfg->rightKori[0], cfg->rightKori[1], cfg->rightKori[2];
-        rightDori << cfg->rightDori[0], cfg->rightDori[1], cfg->rightDori[2];
-
-        knull = cfg->knull;
-        dnull = cfg->dnull;
-
-
-        timeDuration = cfg->timeDuration;
-
-        maxLinearVel = cfg->maxLinearVel;
-        maxAngularVel = cfg->maxAngularVel;
-
-        NJointBimanualCCDMPVelocityControllerInterfaceData initInterfaceData;
-        initInterfaceData.currentLeftPose = Eigen::Matrix4f::Identity();
-        initInterfaceData.currentRightPose = Eigen::Matrix4f::Identity();
-        initInterfaceData.currentLeftJointVals.setZero(leftTargets.size());
-        initInterfaceData.currentRightJointVals.setZero(rightTargets.size());
-        interfaceData.reinitAllBuffers(initInterfaceData);
-
-        NJointBimanualCCDMPVelocityControllerSensorData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentLeftPose = Eigen::Matrix4f::Identity();
-        initSensorData.currentRightPose = Eigen::Matrix4f::Identity();
-        controllerSensorData.reinitAllBuffers(initSensorData);
-    }
-
-    std::string NJointBimanualCCDMPVelocityController::getClassName(const Ice::Current&) const
-    {
-        return "NJointBimanualCCDMPVelocityController";
-    }
-
-    void NJointBimanualCCDMPVelocityController::rtPreActivateController()
-    {
-        NJointBimanualCCDMPVelocityControllerControlData initData;
-        initData.leftTargetVel.resize(6);
-        initData.leftTargetVel.setZero();
-        initData.rightTargetVel.resize(6);
-        initData.rightTargetVel.setZero();
-        initData.leftTargetPose = tcpLeft->getPoseInRootFrame();
-        initData.rightTargetPose = tcpRight->getPoseInRootFrame();
-        initData.virtualTime = cfg->timeDuration;
-        reinitTripleBuffer(initData);
-    }
-
-
-    void NJointBimanualCCDMPVelocityController::controllerRun()
-    {
-        if (!started)
-        {
-            return;
-        }
-
-        if (!controllerSensorData.updateReadBuffer())
-        {
-            return;
-        }
-        double deltaT = controllerSensorData.getReadBuffer().deltaT;
-
-
-        Eigen::VectorXf leftTargetVel;
-        leftTargetVel.resize(6);
-        leftTargetVel.setZero();
-        Eigen::VectorXf rightTargetVel;
-        rightTargetVel.resize(6);
-        rightTargetVel.setZero();
-        Eigen::Matrix4f leftTargetPose;
-        Eigen::Matrix4f rightTargetPose;
-
-        std::vector<TaskSpaceDMPControllerPtr > currentControlGroup;
-        Eigen::Matrix4f currentLeaderPose;
-        Eigen::Matrix4f currentFollowerPose;
-        Eigen::VectorXf currentLeaderTwist;
-        Eigen::VectorXf currentFollowerTwist;
-
-
-
-        // get desired joint values
-        if (leaderName == "Both")
-        {
-            currentControlGroup = bothLeaderGroup;
-
-            TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
-            TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
-            leaderDMPleft->flow(deltaT, controllerSensorData.getReadBuffer().currentLeftPose, controllerSensorData.getReadBuffer().currentLeftTwist);
-            leaderDMPright->flow(deltaT, controllerSensorData.getReadBuffer().currentRightPose, controllerSensorData.getReadBuffer().currentRightTwist);
-
-            leftTargetVel = leaderDMPleft->getTargetVelocity();
-            leftTargetPose = leaderDMPleft->getTargetPoseMat();
-            rightTargetVel = leaderDMPright->getTargetVelocity();
-            rightTargetPose = leaderDMPright->getTargetPoseMat();
-
-            virtualtimer = leaderDMPleft->canVal;
-        }
-        else
-        {
-            if (leaderName == "Left")
-            {
-                currentControlGroup = leftGroup;
-                currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
-                currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
-                currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
-                currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
-            }
-            else if (leaderName == "Right")
-            {
-                currentControlGroup = rightGroup;
-                currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
-                currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
-                currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
-                currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
-            }
-
-            TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
-            TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
-            virtualtimer = leaderDMP->canVal;
-
-            if (virtualtimer < 1e-8)
-            {
-                finished = true;
-                started = false;
-                isDMPRun = false;
-            }
-
-
-            leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
-
-            Eigen::Matrix4f currentFollowerLocalPose;
-            currentFollowerLocalPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0).inverse() * currentFollowerPose.block<3, 3>(0, 0);
-            currentFollowerLocalPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0).inverse() * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
-            followerDMP->flow(deltaT, currentFollowerLocalPose, currentFollowerTwist);
-
-            Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
-            Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
-            Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
-            std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
-
-            Eigen::Matrix4f followerTargetPose;
-            followerTargetPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 3>(0, 0);
-            followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
-
-
-            Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
-            Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
-            followerTargetVel.setZero();
-
-            std::vector<double> leftDMPTarget;
-            std::vector<double> rightDMPTarget;
-
-            if (leaderName == "Left")
-            {
-                leftTargetVel = leaderTargetVel;
-                rightTargetVel = followerTargetVel;
-                leftDMPTarget = leaderDMP->getTargetPose();
-                rightDMPTarget = followerLocalTargetPoseVec;
-                leftTargetPose = leaderTargetPose;
-                rightTargetPose = followerTargetPose;
-            }
-            else if (leaderName == "Right")
-            {
-                rightTargetVel = leaderTargetVel;
-                leftTargetVel = followerTargetVel;
-                rightDMPTarget = leaderDMP->getTargetPose();
-                leftDMPTarget = followerLocalTargetPoseVec;
-                rightTargetPose = leaderTargetPose;
-                leftTargetPose = followerTargetPose;
-            }
-        }
-
-
-        Eigen::VectorXf leftDesiredJoint = leftDesiredJointValues;
-        Eigen::VectorXf rightDesiredJoint = rightDesiredJointValues;
-        if (isLeftJointLearned)
-        {
-            DMP::DVec targetJointState;
-            currentLeftJointState = leftJointDMP->calculateDirectlyVelocity(currentLeftJointState, virtualtimer / timeDuration, deltaT / timeDuration, targetJointState);
-            for (size_t i = 0; i < targetJointState.size(); ++i)
-            {
-                leftDesiredJoint(i) = targetJointState[i];
-            }
-        }
-
-        if (isRightJointLearned)
-        {
-            DMP::DVec targetJointState;
-            currentRightJointState = rightJointDMP->calculateDirectlyVelocity(currentRightJointState, virtualtimer / timeDuration, deltaT / timeDuration, targetJointState);
-            for (size_t i = 0; i < targetJointState.size(); ++i)
-            {
-                rightDesiredJoint(i) = targetJointState[i];
-            }
-        }
-
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().leftTargetVel = leftTargetVel;
-        getWriterControlStruct().rightTargetVel = rightTargetVel;
-        getWriterControlStruct().leftTargetPose = leftTargetPose;
-        getWriterControlStruct().rightTargetPose = rightTargetPose;
-        getWriterControlStruct().leftDesiredJoint = leftDesiredJoint;
-        getWriterControlStruct().rightDesiredJoint = rightDesiredJoint;
-        getWriterControlStruct().virtualTime = virtualtimer;
-        writeControlStruct();
-        isDMPRun = true;
-    }
-
-    Eigen::VectorXf NJointBimanualCCDMPVelocityController::getControlWrench(const Eigen::VectorXf& tcptwist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose)
-    {
-        return Eigen::Vector6f::Zero();
-    }
-
-
-
-    void NJointBimanualCCDMPVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-        controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
-        controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
-        controllerSensorData.getWriteBuffer().deltaT = deltaT;
-        controllerSensorData.getWriteBuffer().currentTime += deltaT;
-
-        // cartesian vel controller
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-
-        Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf leftqpos;
-        Eigen::VectorXf leftqvel;
-        leftqpos.resize(leftPositionSensors.size());
-        leftqvel.resize(leftVelocitySensors.size());
-        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
-        {
-            leftqpos(i) = leftPositionSensors[i]->position;
-            leftqvel(i) = leftVelocitySensors[i]->velocity;
-        }
-
-        Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-        Eigen::VectorXf rightqpos;
-        Eigen::VectorXf rightqvel;
-        rightqpos.resize(rightPositionSensors.size());
-        rightqvel.resize(rightVelocitySensors.size());
-
-        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
-        {
-            rightqpos(i) = rightPositionSensors[i]->position;
-            rightqvel(i) = rightVelocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
-        Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
-        controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
-        controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
-        controllerSensorData.commitWrite();
-
-
-        interfaceData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
-        interfaceData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
-        interfaceData.getWriteBuffer().currentLeftJointVals = leftqpos;
-        interfaceData.getWriteBuffer().currentRightJointVals = rightqpos;
-        interfaceData.commitWrite();
-
-        if (!isDMPRun)
-        {
-            for (size_t i = 0; i < leftTargets.size(); ++i)
-            {
-                leftTargets.at(i)->velocity = 0;
-            }
-            for (size_t i = 0; i < rightTargets.size(); ++i)
-            {
-                rightTargets.at(i)->velocity = 0;
-            }
-        }
-        else
-        {
-            Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
-            Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
-            double virtualtime = rtGetControlStruct().virtualTime;
-            Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
-            Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
-            Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
-            Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
-            Eigen::VectorXf leftDesiredJoint = rtGetControlStruct().leftDesiredJoint;
-            Eigen::VectorXf rightDesiredJoint = rtGetControlStruct().rightDesiredJoint;
-
-            // generate joint control signals
-            Eigen::VectorXf leftCartesianTarget(6);
-            {
-                Eigen::Vector3f targetTCPLinearVelocity;
-                targetTCPLinearVelocity << leftTargetVel(0), leftTargetVel(1), leftTargetVel(2);
-                Eigen::Vector3f currentTCPLinearVelocity;
-                currentTCPLinearVelocity <<  currentLeftTwist(0), currentLeftTwist(1), currentLeftTwist(2);
-                Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
-                Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
-                Eigen::Vector3f posVel = leftKpos.cwiseProduct(desiredPosition - currentTCPPosition) + leftDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-
-
-                Eigen::Vector3f currentTCPAngularVelocity;
-                currentTCPAngularVelocity << currentLeftTwist(3),   currentLeftTwist(4),  currentLeftTwist(5);
-                Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
-                Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-                Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-                Eigen::Vector3f oriVel = leftKori.cwiseProduct(rpy) - leftDori.cwiseProduct(currentTCPAngularVelocity);
-
-                float normLinearVelocity = posVel.norm();
-                if (normLinearVelocity > maxLinearVel)
-                {
-                    posVel = maxLinearVel * posVel / normLinearVelocity;
-                }
-                float normAngularVelocity = oriVel.norm();
-                if (normAngularVelocity > maxAngularVel)
-                {
-                    oriVel = maxAngularVel * oriVel / normAngularVelocity;
-                }
-
-                leftCartesianTarget << posVel, oriVel;
-            }
-
-            Eigen::VectorXf rightCartesianTarget(6);
-            {
-                Eigen::Vector3f targetTCPLinearVelocity;
-                targetTCPLinearVelocity << rightTargetVel(0), rightTargetVel(1), rightTargetVel(2);
-                Eigen::Vector3f currentTCPLinearVelocity;
-                currentTCPLinearVelocity <<  currentRightTwist(0), currentRightTwist(1), currentRightTwist(2);
-                Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
-                Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
-                Eigen::Vector3f posVel = rightKpos.cwiseProduct(desiredPosition - currentTCPPosition) + rightDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-
-
-                Eigen::Vector3f currentTCPAngularVelocity;
-                currentTCPAngularVelocity << currentRightTwist(3),   currentRightTwist(4),  currentRightTwist(5);
-                Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
-                Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-                Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-                Eigen::Vector3f oriVel = rightKori.cwiseProduct(rpy) - rightDori.cwiseProduct(currentTCPAngularVelocity);
-
-                float normLinearVelocity = posVel.norm();
-                if (normLinearVelocity > maxLinearVel)
-                {
-                    posVel = maxLinearVel * posVel / normLinearVelocity;
-                }
-                float normAngularVelocity = oriVel.norm();
-                if (normAngularVelocity > maxAngularVel)
-                {
-                    oriVel = maxAngularVel * oriVel / normAngularVelocity;
-                }
-
-                rightCartesianTarget << posVel, oriVel;
-
-            }
-
-            Eigen::VectorXf leftNullspaceVel = knull * (leftDesiredJoint - leftqpos) - dnull * leftqvel;
-            Eigen::VectorXf rightNullspaceVel = knull * (rightDesiredJoint - rightqpos) - dnull * rightqvel;
-            Eigen::VectorXf desiredLeftVels = leftCtrl->calculate(leftCartesianTarget, leftNullspaceVel, VirtualRobot::IKSolver::CartesianSelection::All);
-            Eigen::VectorXf desiredRightVels = rightCtrl->calculate(rightCartesianTarget, rightNullspaceVel, VirtualRobot::IKSolver::CartesianSelection::All);
-
-            // torque limit
-            for (size_t i = 0; i < leftTargets.size(); ++i)
-            {
-
-                float desiredVel = desiredLeftVels[i];
-
-                if (fabs(desiredVel) > cfg->maxJointVel)
-                {
-                    desiredVel = cfg->maxJointVel * desiredVel / fabs(desiredVel);
-                }
-
-                leftTargets.at(i)->velocity = desiredVel;
-                debugDataInfo.getWriteBuffer().desired_velocities[leftJointNames[i]] = desiredVel;
-
-            }
-
-            for (size_t i = 0; i < rightTargets.size(); ++i)
-            {
-                float desiredVel = desiredRightVels[i];
-
-                if (fabs(desiredVel) > cfg->maxJointVel)
-                {
-                    desiredVel = cfg->maxJointVel * desiredVel / fabs(desiredVel);
-                }
-
-                rightTargets.at(i)->velocity = desiredVel;
-                debugDataInfo.getWriteBuffer().desired_velocities[rightJointNames[i]] = desiredVel;
-            }
-
-
-            debugDataInfo.getWriteBuffer().leftControlSignal_x = leftCartesianTarget(0);
-            debugDataInfo.getWriteBuffer().leftControlSignal_y = leftCartesianTarget(1);
-            debugDataInfo.getWriteBuffer().leftControlSignal_z = leftCartesianTarget(2);
-            debugDataInfo.getWriteBuffer().leftControlSignal_ro = leftCartesianTarget(3);
-            debugDataInfo.getWriteBuffer().leftControlSignal_pi = leftCartesianTarget(4);
-            debugDataInfo.getWriteBuffer().leftControlSignal_ya = leftCartesianTarget(5);
-
-            debugDataInfo.getWriteBuffer().rightControlSignal_x = rightCartesianTarget(0);
-            debugDataInfo.getWriteBuffer().rightControlSignal_y = rightCartesianTarget(1);
-            debugDataInfo.getWriteBuffer().rightControlSignal_z = rightCartesianTarget(2);
-            debugDataInfo.getWriteBuffer().rightControlSignal_ro = rightCartesianTarget(3);
-            debugDataInfo.getWriteBuffer().rightControlSignal_pi = rightCartesianTarget(4);
-            debugDataInfo.getWriteBuffer().rightControlSignal_ya = rightCartesianTarget(5);
-
-            debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
-            debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
-            debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
-            debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
-            debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
-            debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
-
-
-            debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
-            debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
-            debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
-
-            debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
-            debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
-            debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
-            debugDataInfo.getWriteBuffer().virtualTime = virtualtime;
-
-            debugDataInfo.commitWrite();
-        }
-
-    }
-
-    void NJointBimanualCCDMPVelocityController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        if (name == "LeftLeader")
-        {
-            leftGroup.at(0)->learnDMPFromFiles(fileNames);
-        }
-        else if (name == "LeftFollower")
-        {
-            rightGroup.at(1)->learnDMPFromFiles(fileNames);
-        }
-        else if (name == "RightLeader")
-        {
-            rightGroup.at(0)->learnDMPFromFiles(fileNames);
-        }
-        else if (name == "RightFollower")
-        {
-            leftGroup.at(1)->learnDMPFromFiles(fileNames);
-        }
-        else if (name == "LeftJoint")
-        {
-            DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-            DMP::SampledTrajectoryV2 traj;
-            std::string absPath;
-            ArmarXDataPath::getAbsolutePath(fileNames.at(0), absPath);
-            traj.readFromCSVFile(absPath);
-            traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
-            trajs.push_back(traj);
-            leftJointDMP->learnFromTrajectories(trajs);
-        }
-        else if (name == "RightJoint")
-        {
-            DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-            DMP::SampledTrajectoryV2 traj;
-            std::string absPath;
-            ArmarXDataPath::getAbsolutePath(fileNames.at(0), absPath);
-            traj.readFromCSVFile(absPath);
-            traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
-            trajs.push_back(traj);
-            rightJointDMP->learnFromTrajectories(trajs);
-        }
-        else
-        {
-            ARMARX_ERROR << "The given name is not supported by CCDMP, the supported names contain ";
-        }
-
-    }
-
-    void NJointBimanualCCDMPVelocityController::learnDMPFromBothFiles(const Ice::StringSeq& leftFiles, const Ice::StringSeq& rightFiles, const Ice::Current&)
-    {
-        ARMARX_CHECK_EQUAL(leftFiles.size(), rightFiles.size());
-
-        DMP::Vec<DMP::SampledTrajectoryV2 > leftFollowerTrajs;
-        DMP::Vec<DMP::SampledTrajectoryV2 > rightFollowerTrajs;
-
-        for (size_t i = 0; i < leftFiles.size(); ++i)
-        {
-            DMP::SampledTrajectoryV2 leftFollowerTraj = DMP::RobotHelpers::getRelativeTrajFromFiles(leftFiles[i], rightFiles[i]);
-            leftFollowerTrajs.push_back(leftFollowerTraj);
-            DMP::SampledTrajectoryV2 rightFollowerTraj = DMP::RobotHelpers::getRelativeTrajFromFiles(rightFiles[i], leftFiles[i]);
-            rightFollowerTrajs.push_back(rightFollowerTraj);
-        }
-
-        leftGroup.at(0)->learnDMPFromFiles(leftFiles, cfg->initRatio);
-        leftGroup.at(1)->learnDMPFromSampledTrajectory(leftFollowerTrajs, cfg->initRatio);
-        rightGroup.at(0)->learnDMPFromFiles(rightFiles, cfg->initRatio);
-        rightGroup.at(1)->learnDMPFromSampledTrajectory(rightFollowerTrajs, cfg->initRatio);
-    }
-
-    void NJointBimanualCCDMPVelocityController::setRatios(const Ice::DoubleSeq& ratios, const Ice::Current&)
-    {
-        leftGroup.at(0)->setRatios(ratios);
-        leftGroup.at(1)->setRatios(ratios);
-        rightGroup.at(0)->setRatios(ratios);
-        rightGroup.at(1)->setRatios(ratios);
-    }
-
-
-    void NJointBimanualCCDMPVelocityController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-        if (leaderName == "Left")
-        {
-            leftGroup.at(0)->setViaPose(u, viapoint);
-        }
-        else
-        {
-            rightGroup.at(0)->setViaPose(u, viapoint);
-        }
-    }
-
-    void NJointBimanualCCDMPVelocityController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard(controllerMutex);
-        if (leaderName == "Left")
-        {
-            leftGroup.at(0)->setGoalPoseVec(goals);
-        }
-        else
-        {
-            rightGroup.at(0)->setGoalPoseVec(goals);
-        }
-
-    }
-
-    void NJointBimanualCCDMPVelocityController::changeLeader(const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-
-        if (leaderName == "Left")
-        {
-            leaderName = "Right";
-            rightGroup.at(0)->canVal = virtualtimer;
-            rightGroup.at(1)->canVal = virtualtimer;
-        }
-        else
-        {
-            leaderName = "Left";
-            leftGroup.at(0)->canVal = virtualtimer;
-            leftGroup.at(1)->canVal = virtualtimer;
-        }
-
-    }
-
-
-    void NJointBimanualCCDMPVelocityController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
-    {
-        ARMARX_INFO << "start running ccdmp";
-        while (!interfaceData.updateReadBuffer())
-        {
-            usleep(100);
-        }
-        Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().currentLeftPose;
-        Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().currentRightPose;
-        Eigen::VectorXf leftJointVals = interfaceData.getReadBuffer().currentLeftJointVals;
-        Eigen::VectorXf rightJointVals = interfaceData.getReadBuffer().currentRightJointVals;
-
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            DMP::DMPState jstate;
-            jstate.pos = leftJointVals(i);
-            jstate.vel = 0;
-            currentLeftJointState.push_back(jstate);
-        }
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            DMP::DMPState jstate;
-            jstate.pos = rightJointVals(i);
-            jstate.vel = 0;
-            currentRightJointState.push_back(jstate);
-        }
-
-        virtualtimer = cfg->timeDuration;
-        leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
-        rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
-
-
-        ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
-
-        leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
-        rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
-
-        finished = false;
-        started = true;
-    }
-
-    Eigen::Matrix4f NJointBimanualCCDMPVelocityController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
-    {
-        Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
-
-        localPose.block<3, 3>(0, 0) = newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
-        localPose.block<3, 1>(0, 3) = newCoordinate.block<3, 3>(0, 0).inverse() * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
-
-
-        return localPose;
-    }
-
-
-    void NJointBimanualCCDMPVelocityController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-
-        StringVariantBaseMap datafields;
-        auto values = debugDataInfo.getUpToDateReadBuffer().desired_velocities;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
-        for (auto& pair : constrained_force)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-
-        datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
-        datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
-        datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
-        datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
-        datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
-        datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
-
-
-        datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
-        datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
-        datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
-        datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
-        datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
-        datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
-
-        datafields["leftControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_x);
-        datafields["leftControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_y);
-        datafields["leftControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_z);
-        datafields["leftControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ro);
-        datafields["leftControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_pi);
-        datafields["leftControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ya);
-
-
-        datafields["rightControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_x);
-        datafields["rightControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_y);
-        datafields["rightControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_z);
-        datafields["rightControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ro);
-        datafields["rightControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_pi);
-        datafields["rightControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ya);
-
-        datafields["virtual_timer"] = new Variant(debugDataInfo.getUpToDateReadBuffer().virtualTime);
-
-
-        debugObs->setDebugChannel("CCDMPVelocityController", datafields);
-    }
-
-    void NJointBimanualCCDMPVelocityController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        started = false;
-        runTask("NJointBimanualCCDMPVelocityController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-        ARMARX_INFO << "Finished init ...";
-
-    }
-
-    void NJointBimanualCCDMPVelocityController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h
deleted file mode 100644
index 6d5a4e275555a3d38cf41db9412a53874b5dbed7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h
+++ /dev/null
@@ -1,266 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-
-#include <dmp/general/simoxhelpers.h>
-namespace armarx
-{
-    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPVelocityController);
-    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPVelocityControllerControlData);
-
-    using ViaPoint = std::pair<double, DMP::DVec >;
-    using ViaPointsSet = std::vector<ViaPoint >;
-    class NJointBimanualCCDMPVelocityControllerControlData
-    {
-    public:
-        Eigen::VectorXf leftTargetVel;
-        Eigen::VectorXf rightTargetVel;
-
-        Eigen::Matrix4f leftTargetPose;
-        Eigen::Matrix4f rightTargetPose;
-
-        double virtualTime;
-
-        Eigen::VectorXf leftDesiredJoint;
-        Eigen::VectorXf rightDesiredJoint;
-    };
-
-    /**
-     * @brief The NJointBimanualCCDMPVelocityController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointBimanualCCDMPVelocityController :
-        public NJointControllerWithTripleBuffer<NJointBimanualCCDMPVelocityControllerControlData>,
-        public NJointBimanualCCDMPVelocityControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointBimanualCCDMPVelocityControllerConfigPtr;
-        NJointBimanualCCDMPVelocityController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const override;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        // NJointBimanualCCDMPVelocityControllerInterface interface
-        void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&) override;
-        void learnDMPFromBothFiles(const Ice::StringSeq&, const Ice::StringSeq&, const Ice::Current&) override;
-
-        bool isFinished(const Ice::Current&) override
-        {
-            return finished;
-        }
-
-        void setRatios(const Ice::DoubleSeq& ratios, const Ice::Current&) override;
-
-        void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&) override;
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
-        void changeLeader(const Ice::Current&) override;
-
-        double getVirtualTime(const Ice::Current&) override
-        {
-            return virtualtimer;
-        }
-
-        std::string getLeaderName(const Ice::Current&) override
-        {
-            return leaderName;
-        }
-
-    protected:
-
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
-        void onInitNJointController() override;
-        void onDisconnectNJointController() override;
-        void controllerRun();
-    private:
-
-        Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose);
-
-        Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
-        Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
-        {
-            Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
-            newCoordinate(0, 3) = newCoordinateVec.at(0);
-            newCoordinate(1, 3) = newCoordinateVec.at(1);
-            newCoordinate(2, 3) = newCoordinateVec.at(2);
-
-            Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
-            globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
-            globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
-            globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
-
-            return getLocalPose(newCoordinate, globalTargetPose);
-
-        }
-
-        struct DebugBufferData
-        {
-            StringFloatDictionary desired_velocities;
-            StringFloatDictionary constrained_force;
-            float leftTargetPose_x;
-            float leftTargetPose_y;
-            float leftTargetPose_z;
-            float rightTargetPose_x;
-            float rightTargetPose_y;
-            float rightTargetPose_z;
-
-            float leftCurrentPose_x;
-            float leftCurrentPose_y;
-            float leftCurrentPose_z;
-            float rightCurrentPose_x;
-            float rightCurrentPose_y;
-            float rightCurrentPose_z;
-
-            float leftControlSignal_x;
-            float leftControlSignal_y;
-            float leftControlSignal_z;
-            float leftControlSignal_ro;
-            float leftControlSignal_pi;
-            float leftControlSignal_ya;
-
-            float rightControlSignal_x;
-            float rightControlSignal_y;
-            float rightControlSignal_z;
-            float rightControlSignal_ro;
-            float rightControlSignal_pi;
-            float rightControlSignal_ya;
-
-            double virtualTime;
-
-        };
-
-        bool finished;
-        TripleBuffer<DebugBufferData> debugDataInfo;
-
-        struct NJointBimanualCCDMPVelocityControllerSensorData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentLeftPose;
-            Eigen::Matrix4f currentRightPose;
-            Eigen::VectorXf currentLeftTwist;
-            Eigen::VectorXf currentRightTwist;
-
-        };
-        TripleBuffer<NJointBimanualCCDMPVelocityControllerSensorData> controllerSensorData;
-
-        struct NJointBimanualCCDMPVelocityControllerInterfaceData
-        {
-            Eigen::Matrix4f currentLeftPose;
-            Eigen::Matrix4f currentRightPose;
-
-            Eigen::VectorXf currentLeftJointVals;
-            Eigen::VectorXf currentRightJointVals;
-        };
-
-        TripleBuffer<NJointBimanualCCDMPVelocityControllerInterfaceData> interfaceData;
-
-
-        std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
-        std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
-        const SensorValueForceTorque* rightForceTorque;
-        const SensorValueForceTorque* leftForceTorque;
-
-        NJointBimanualCCDMPVelocityControllerConfigPtr cfg;
-        VirtualRobot::DifferentialIKPtr leftIK;
-        VirtualRobot::DifferentialIKPtr rightIK;
-
-        std::vector<TaskSpaceDMPControllerPtr > leftGroup;
-        std::vector<TaskSpaceDMPControllerPtr > rightGroup;
-        std::vector<TaskSpaceDMPControllerPtr > bothLeaderGroup;
-        DMP::UMIDMPPtr leftJointDMP;
-        DMP::UMIDMPPtr rightJointDMP;
-        bool isLeftJointLearned;
-        bool isRightJointLearned;
-
-
-        std::string leaderName;
-
-        VirtualRobot::RobotNodePtr tcpLeft;
-        VirtualRobot::RobotNodePtr tcpRight;
-
-        double virtualtimer;
-        double timeDuration;
-
-        mutable MutexType controllerMutex;
-
-        Eigen::VectorXf leftDesiredJointValues;
-        Eigen::VectorXf rightDesiredJointValues;
-
-        Eigen::Vector3f leftKpos;
-        Eigen::Vector3f leftKori;
-        Eigen::Vector3f leftDpos;
-        Eigen::Vector3f leftDori;
-
-        Eigen::Vector3f rightKpos;
-        Eigen::Vector3f rightKori;
-        Eigen::Vector3f rightDpos;
-        Eigen::Vector3f rightDori;
-
-
-        float knull;
-        float dnull;
-
-        std::vector<std::string> leftJointNames;
-        std::vector<std::string> rightJointNames;
-
-        float torqueLimit;
-        VirtualRobot::RobotNodeSetPtr leftRNS;
-        VirtualRobot::RobotNodeSetPtr rightRNS;
-
-        Eigen::VectorXf leftNullSpaceCoefs;
-        Eigen::VectorXf rightNullSpaceCoefs;
-
-
-        float maxLinearVel;
-        float maxAngularVel;
-
-
-        CartesianVelocityControllerPtr leftCtrl;
-        CartesianVelocityControllerPtr rightCtrl;
-
-        bool started;
-        bool isDMPRun;
-        DMP::Vec<DMP::DMPState> currentLeftJointState;
-        DMP::Vec<DMP::DMPState> currentRightJointState;
-
-        // NJointBimanualCCDMPVelocityControllerInterface interface
-
-        // NJointController interface
-    protected:
-        void rtPreActivateController() override;
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.cpp
deleted file mode 100644
index 7112666794b57434aeee754772966f7e437bfb7d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.cpp
+++ /dev/null
@@ -1,1098 +0,0 @@
-#include "NJointBimanualCCDMPController.h"
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointBimanualCCDMPController> registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController");
-
-    NJointBimanualCCDMPController::NJointBimanualCCDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "Preparing ... ";
-        cfg = NJointBimanualCCDMPControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_EXPRESSION(prov);
-        RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
-        ARMARX_CHECK_EXPRESSION(robotUnit);
-
-        leftRNS = robotUnit->getRtRobot()->getRobotNodeSet("LeftArm");
-        rnsLeftBody = robotUnit->getRtRobot()->getRobotNodeSet("LeftArmCol");
-        for (size_t i = 0; i < leftRNS->getSize(); ++i)
-        {
-            std::string jointName = leftRNS->getNode(i)->getName();
-
-            leftJointNames.push_back(jointName);
-            ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = prov->getSensorValue(jointName);
-            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-            if (!accelerationSensor)
-            {
-                ARMARX_WARNING << "No accelerationSensor available for " << jointName;
-            }
-
-
-            leftVelocitySensors.push_back(velocitySensor);
-            leftPositionSensors.push_back(positionSensor);
-            leftAccelerationSensors.push_back(accelerationSensor);
-
-        };
-
-
-
-        rnsRightBody = robotUnit->getRtRobot()->getRobotNodeSet("RightArmCol");
-
-        rightRNS = robotUnit->getRtRobot()->getRobotNodeSet("RightArm");
-        for (size_t i = 0; i < rightRNS->getSize(); ++i)
-        {
-            std::string jointName = rightRNS->getNode(i)->getName();
-            rightJointNames.push_back(jointName);
-            ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = prov->getSensorValue(jointName);
-            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-            if (!accelerationSensor)
-            {
-                ARMARX_WARNING << "No accelerationSensor available for " << jointName;
-            }
-
-            rightVelocitySensors.push_back(velocitySensor);
-            rightPositionSensors.push_back(positionSensor);
-            rightAccelerationSensors.push_back(accelerationSensor);
-
-        };
-
-
-        const SensorValueBase* svlf = prov->getSensorValue("FT L");
-        leftForceTorque = svlf->asA<SensorValueForceTorque>();
-        const SensorValueBase* svrf = prov->getSensorValue("FT R");
-        rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
-        leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
-        taskSpaceDMPConfig.DMPAmplitude = 1.0;
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-
-        TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig));
-        TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig));
-        TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig));
-        TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig));
-
-        leftGroup.push_back(leftLeader);
-        leftGroup.push_back(rightFollower);
-
-        rightGroup.push_back(rightLeader);
-        rightGroup.push_back(leftFollower);
-
-        bothLeaderGroup.push_back(leftLeader);
-        bothLeaderGroup.push_back(rightLeader);
-
-
-        tcpLeft = leftRNS->getTCP();
-        tcpRight = rightRNS->getTCP();
-        NJointBimanualCCDMPControllerSensorData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentLeftPose = leftRNS->getTCP()->getPoseInRootFrame();
-        initSensorData.currentRightPose = rightRNS->getTCP()->getPoseInRootFrame();
-        controllerSensorData.reinitAllBuffers(initSensorData);
-
-        leaderName = "both";
-
-        NJointBimanualCCDMPControllerControlData initData;
-        initData.leftTargetVel.resize(6);
-        initData.leftTargetVel.setZero();
-        initData.rightTargetVel.resize(6);
-        initData.rightTargetVel.setZero();
-        reinitTripleBuffer(initData);
-
-        leftDesiredJointValues.resize(leftTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
-        }
-
-        rightDesiredJointValues.resize(rightTargets.size());
-        ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
-        }
-
-        KoriFollower = cfg->KoriFollower;
-        KposFollower = cfg->KposFollower;
-
-        kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
-        dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
-        kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
-        dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
-
-        knull = cfg->knull;
-        dnull = cfg->dnull;
-
-        torqueLimit = cfg->torqueLimit;
-
-        kpf.resize(12);
-        kif.resize(12);
-        forceC_des.resize(12);
-
-        ARMARX_CHECK_EQUAL(cfg->Kpf.size(), kpf.rows());
-        ARMARX_CHECK_EQUAL(cfg->Kif.size(), kif.rows());
-        ARMARX_CHECK_EQUAL(cfg->DesiredForce.size(), forceC_des.rows());
-
-
-        for (size_t i = 0; i < 12; i++)
-        {
-            kpf(i) = cfg->Kpf.at(i);
-            kif(i) = cfg->Kif.at(i);
-            forceC_des(i) = cfg->DesiredForce.at(i);
-        }
-
-        boxWidth = cfg->BoxWidth;
-
-        filtered_leftForce.setZero();
-        filtered_leftTorque.setZero();
-        filtered_rightForce.setZero();
-        filtered_rightTorque.setZero();
-
-        //        offset_leftForce.setZero();
-        //        offset_leftTorque.setZero();
-        //        offset_rightForce.setZero();
-        //        offset_rightTorque.setZero();
-
-        offset_leftForce(0) = -4.34;
-        offset_leftForce(1) = -8.21;
-        offset_leftForce(2) = 15.59;
-
-        offset_leftTorque(0) = 1.42;
-        offset_leftTorque(1) = 0.29;
-        offset_leftTorque(2) = 0.09;
-
-        offset_rightForce(0) = 2.88;
-        offset_rightForce(1) = 11.15;
-        offset_rightForce(2) = -20.18;
-
-        offset_rightTorque(0) = -1.83;
-        offset_rightTorque(1) = 0.34;
-        offset_rightTorque(2) = -0.01;
-
-
-        filterTimeConstant = cfg->FilterTimeConstant;
-
-        ftPIDController.resize(12);
-        for (size_t i = 0; i < ftPIDController.size(); i++)
-        {
-            ftPIDController[i].reset(new PIDController(kpf(i), kif(i), 0.0, 10, 10));
-        }
-
-        isForceCompensateDone = false;
-
-    }
-
-    std::string NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointBimanualCCDMPController";
-    }
-
-    void NJointBimanualCCDMPController::controllerRun()
-    {
-        if (!controllerSensorData.updateReadBuffer())
-        {
-            return;
-        }
-
-        double deltaT = controllerSensorData.getReadBuffer().deltaT;
-
-
-        Eigen::VectorXf leftTargetVel;
-        leftTargetVel.resize(6);
-        leftTargetVel.setZero();
-        Eigen::VectorXf rightTargetVel;
-        rightTargetVel.resize(6);
-        rightTargetVel.setZero();
-
-        std::vector<TaskSpaceDMPControllerPtr > currentControlGroup;
-        Eigen::Matrix4f currentLeaderPose;
-        Eigen::Matrix4f currentFollowerPose;
-        Eigen::VectorXf currentLeaderTwist;
-        Eigen::VectorXf currentFollowerTwist;
-        if (leaderName == "Left")
-        {
-            currentControlGroup = leftGroup;
-            currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
-            currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
-            currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
-            currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
-        }
-        else if (leaderName == "right")
-        {
-            currentControlGroup = rightGroup;
-            currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
-            currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
-            currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
-            currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
-        }
-        else
-        {
-            currentControlGroup = bothLeaderGroup;
-
-            TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
-            TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
-            leaderDMPleft->flow(deltaT, controllerSensorData.getReadBuffer().currentLeftPose, controllerSensorData.getReadBuffer().currentLeftTwist);
-            leaderDMPright->flow(deltaT, controllerSensorData.getReadBuffer().currentRightPose, controllerSensorData.getReadBuffer().currentRightTwist);
-
-            Eigen::VectorXf leftTargetVel = leaderDMPleft->getTargetVelocity();
-            Eigen::Matrix4f leftTargetPose = leaderDMPleft->getTargetPoseMat();
-            Eigen::VectorXf rightTargetVel = leaderDMPright->getTargetVelocity();
-            Eigen::Matrix4f rightTargetPose = leaderDMPright->getTargetPoseMat();
-
-            virtualtimer = leaderDMPleft->canVal;
-            LockGuardType guard {controlDataMutex};
-            getWriterControlStruct().leftTargetVel = leftTargetVel;
-            getWriterControlStruct().rightTargetVel = rightTargetVel;
-            getWriterControlStruct().leftTargetPose = leftTargetPose;
-            getWriterControlStruct().rightTargetPose = rightTargetPose;
-            writeControlStruct();
-
-            return;
-        }
-
-        TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
-        TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
-
-
-        leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
-
-        Eigen::Matrix4f currentFollowerLocalPose;
-        currentFollowerLocalPose.block<3, 3>(0, 0) = currentFollowerPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0).inverse();
-        currentFollowerLocalPose.block<3, 1>(0, 3) = currentFollowerLocalPose.block<3, 3>(0, 0) * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
-        followerDMP->flow(deltaT, currentFollowerPose, currentFollowerTwist);
-
-        Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
-        Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
-        Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
-        std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
-
-        Eigen::Matrix4f followerTargetPose;
-        followerTargetPose.block<3, 3>(0, 0) = followerLocalTargetPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0);
-        followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
-
-
-        Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
-        Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
-        //        followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
-        followerTargetVel.block<3, 1>(0, 0) = KposFollower * (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
-
-
-        Eigen::Matrix3f followerDiffMat =  followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
-        Eigen::Vector3f followerDiffRPY = KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
-        followerTargetVel(3) = followerDiffRPY(0);
-        followerTargetVel(4) = followerDiffRPY(1);
-        followerTargetVel(5) = followerDiffRPY(2);
-
-        virtualtimer = leaderDMP->canVal;
-
-
-        std::vector<double> leftDMPTarget;
-        std::vector<double> rightDMPTarget;
-
-        Eigen::Matrix4f leftTargetPose;
-        Eigen::Matrix4f rightTargetPose;
-
-        if (leaderName == "Left")
-        {
-            leftTargetVel = leaderTargetVel;
-            rightTargetVel = followerTargetVel;
-
-            leftDMPTarget = leaderDMP->getTargetPose();
-            rightDMPTarget = followerLocalTargetPoseVec;
-
-
-            leftTargetPose = leaderTargetPose;
-            rightTargetPose = followerLocalTargetPose;
-        }
-        else if (leaderName == "right")
-        {
-            rightTargetVel = leaderTargetVel;
-            leftTargetVel = followerTargetVel;
-
-            rightDMPTarget = leaderDMP->getTargetPose();
-            leftDMPTarget = followerLocalTargetPoseVec;
-
-            rightTargetPose = leaderTargetPose;
-            leftTargetPose = followerLocalTargetPose;
-
-        }
-
-
-
-
-
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_xvel"] = leftTargetVel(0);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yvel"] = leftTargetVel(1);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_zvel"] = leftTargetVel(2);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_rollvel"] = leftTargetVel(3);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_pitchvel"] = leftTargetVel(4);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yawvel"] = leftTargetVel(5);
-
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_xvel"] = rightTargetVel(0);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yvel"] = rightTargetVel(1);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_zvel"] = rightTargetVel(2);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_rollvel"] = rightTargetVel(3);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_pitchvel"] = rightTargetVel(4);
-        //        debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yawvel"] = rightTargetVel(5);
-
-
-        //        debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_x"] = leftDMPTarget[0];
-        //        debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_y"] = leftDMPTarget[1];
-        //        debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_z"] = leftDMPTarget[2];
-        //        debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qw"] = leftDMPTarget[3];
-        //        debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qx"] = leftDMPTarget[4];
-        //        debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qy"] = leftDMPTarget[5];
-        //        debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qz"] = leftDMPTarget[6];
-
-        //        debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_x"] = rightDMPTarget[0];
-        //        debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_y"] = rightDMPTarget[1];
-        //        debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_z"] = rightDMPTarget[2];
-        //        debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qw"] = rightDMPTarget[3];
-        //        debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qx"] = rightDMPTarget[4];
-        //        debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qy"] = rightDMPTarget[5];
-        //        debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qz"] = rightDMPTarget[6];
-
-        //        std::vector<double> currentLeftPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentLeftPose);
-        //        std::vector<double> currentRightPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentRightPose);
-
-
-        //        debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = currentLeftPoseVec[0];
-        //        debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = currentLeftPoseVec[1];
-        //        debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = currentLeftPoseVec[2];
-        //        debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = currentLeftPoseVec[3];
-        //        debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = currentLeftPoseVec[4];
-        //        debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = currentLeftPoseVec[5];
-        //        debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = currentLeftPoseVec[6];
-
-        //        debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = currentRightPoseVec[0];
-        //        debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = currentRightPoseVec[1];
-        //        debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = currentRightPoseVec[2];
-        //        debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = currentRightPoseVec[3];
-        //        debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = currentRightPoseVec[4];
-        //        debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = currentRightPoseVec[5];
-        //        debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = currentRightPoseVec[6];
-
-
-        //        debugOutputData.getWriteBuffer().leaderCanVal = leaderDMP->debugData.canVal;
-        //        debugOutputData.getWriteBuffer().leadermpcFactor =  leaderDMP->debugData.mpcFactor;
-        //        debugOutputData.getWriteBuffer().leadererror = leaderDMP->debugData.poseError;
-        //        debugOutputData.getWriteBuffer().leaderposError = leaderDMP->debugData.posiError;
-        //        debugOutputData.getWriteBuffer().leaderoriError = leaderDMP->debugData.oriError;
-
-        //        debugOutputData.getWriteBuffer().followerCanVal = followerDMP->debugData.canVal;
-        //        debugOutputData.getWriteBuffer().followermpcFactor =  followerDMP->debugData.mpcFactor;
-        //        debugOutputData.getWriteBuffer().followererror = followerDMP->debugData.poseError;
-        //        debugOutputData.getWriteBuffer().followerposError = followerDMP->debugData.posiError;
-        //        debugOutputData.getWriteBuffer().followeroriError = followerDMP->debugData.oriError;
-
-        //        debugOutputData.commitWrite();
-
-
-
-
-
-
-
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().leftTargetVel = leftTargetVel;
-        getWriterControlStruct().rightTargetVel = rightTargetVel;
-        getWriterControlStruct().leftTargetPose = leftTargetPose;
-        getWriterControlStruct().rightTargetPose = rightTargetPose;
-        writeControlStruct();
-    }
-
-    Eigen::VectorXf NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf& tcptwist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose)
-    {
-        //        Eigen::Vector3f currentTCPLinearVelocity;
-        //        currentTCPLinearVelocity << 0.001 * tcptwist(0),   0.001 * tcptwist(1), 0.001 * tcptwist(2);
-        //        Eigen::Vector3f currentTCPAngularVelocity;
-        //        currentTCPAngularVelocity << tcptwist(3),   tcptwist(4),  tcptwist(5);
-
-        //        Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
-        //        Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
-        //        Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
-        //        Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
-
-        //        Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
-        //        Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
-        //        Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-        //        Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
-        //        Eigen::Vector6f tcpDesiredWrench;
-        //        tcpDesiredWrench <<   0.001 * tcpDesiredForce, tcpDesiredTorque;
-
-        //        return tcpDesiredWrench;
-
-    }
-
-
-
-    void NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-        controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
-        controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
-        controllerSensorData.getWriteBuffer().deltaT = deltaT;
-        controllerSensorData.getWriteBuffer().currentTime += deltaT;
-
-
-        //        if(controllerSensorData.getWriteBuffer().currentTime > 0.3 && !isForceCompensateDone)
-        //        {
-        //            offset_leftForce = filtered_leftForce;
-        //            offset_leftTorque = filtered_leftTorque;
-        //            offset_rightForce = filtered_rightForce;
-        //            offset_rightTorque = filtered_rightTorque;
-        //            isForceCompensateDone = true;
-
-
-        //            filtered_leftForce.setZero();
-        //            filtered_leftTorque.setZero();
-        //            filtered_rightForce.setZero();
-        //            filtered_rightTorque.setZero();
-        //        }
-
-        //        if(controllerSensorData.getWriteBuffer().currentTime <= 0.3)
-        //        {
-        //            // torque limit
-        //            for (size_t i = 0; i < leftTargets.size(); ++i)
-        //            {
-        //                leftTargets.at(i)->torque = 0;
-        //            }
-
-        //            for (size_t i = 0; i < rightTargets.size(); ++i)
-        //            {
-        //                rightTargets.at(i)->torque = 0;
-        //            }
-
-        //            return;
-        //        }
-
-        // cartesian vel controller
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-
-        Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-        jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
-
-        Eigen::VectorXf leftqpos;
-        Eigen::VectorXf leftqvel;
-        leftqpos.resize(leftPositionSensors.size());
-        leftqvel.resize(leftVelocitySensors.size());
-        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
-        {
-            leftqpos(i) = leftPositionSensors[i]->position;
-            leftqvel(i) = leftVelocitySensors[i]->velocity;
-        }
-
-        Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-        jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
-
-        Eigen::VectorXf rightqpos;
-        Eigen::VectorXf rightqvel;
-        rightqpos.resize(rightPositionSensors.size());
-        rightqvel.resize(rightVelocitySensors.size());
-
-        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
-        {
-            rightqpos(i) = rightPositionSensors[i]->position;
-            rightqvel(i) = rightVelocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
-        Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
-
-        controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
-        controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
-        controllerSensorData.commitWrite();
-
-
-        Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
-        Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
-        Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
-        Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
-        Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
-
-        // Todo: force control
-        int nl = leftRNS->getSize();
-        int nr = rightRNS->getSize();
-        int n = nl + nr;
-        // GraspMatrix
-
-        //        Eigen::Matrix4f poseLeftInRightCoordinate = tcpRight->toLocalCoordinateSystem(tcpLeft->getGlobalPose());
-        //        Eigen::Vector3f positionLeftInRightCoordinate;
-        //        positionLeftInRightCoordinate << poseLeftInRightCoordinate(0,3), poseLeftInRightCoordinate(1,3), poseLeftInRightCoordinate(2,3);
-        //        boxWidth = 0.001 * 0.5 * positionLeftInRightCoordinate.norm();
-
-
-        Eigen::Vector3f localDistanceCoM;
-        localDistanceCoM << 0.5 * boxWidth, 0, 0;
-
-
-        Eigen::Vector3f rl = -leftCurrentPose.block<3, 3>(0, 0) * localDistanceCoM; //tcpLeft->getRobot()->getRootNode()->toLocalCoordinateSystem(tcpLeft->toGlobalCoordinateSystemVec(localDistanceCoM));
-        Eigen::Vector3f rr = rightCurrentPose.block<3, 3>(0, 0) * localDistanceCoM;
-        Eigen::MatrixXf leftGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
-        Eigen::MatrixXf rightGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
-        leftGraspMatrix.block<3, 3>(3, 0) << 0, -rl(2), rl(1),
-                              rl(2), 0, -rl(0),
-                              -rl(1), rl(0), 0;
-        rightGraspMatrix.block<3, 3>(3, 0) << 0, -rr(2), rr(1),
-                               rr(2), 0, -rr(0),
-                               -rr(1), rr(0), 0;
-        Eigen::MatrixXf graspMatrix;
-        graspMatrix.resize(6, 12);
-        graspMatrix << leftGraspMatrix, rightGraspMatrix;
-
-        // constraint Jacobain and projection matrix
-        Eigen::MatrixXf jacobi;
-        jacobi.resize(12, n);
-        jacobi.setZero(12, n);
-        jacobi.block < 6, 8 > (0, 0) = jacobiL;
-        jacobi.block < 6, 8 > (6, 8) = jacobiR;
-
-        Eigen::MatrixXf pinvGraspMatrix = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 1);
-        Eigen::MatrixXf proj2GraspNullspace = Eigen::MatrixXf::Identity(12, 12) - pinvGraspMatrix * graspMatrix;
-        Eigen::MatrixXf jacobiConstrained = proj2GraspNullspace * jacobi;
-
-        Eigen::MatrixXf pinvJacobiC = leftIK->computePseudoInverseJacobianMatrix(jacobiConstrained, 1);
-        Eigen::MatrixXf projection = Eigen::MatrixXf::Identity(n, n) - pinvJacobiC * jacobiConstrained;
-
-
-
-        // calculate inertia matrix
-        Eigen::MatrixXf leftInertialMatrix;
-        Eigen::MatrixXf rightInertialMatrix;
-        leftInertialMatrix.setZero(nl, nl);
-        rightInertialMatrix.setZero(nr, nr);
-
-        for (size_t i = 0; i < rnsLeftBody->getNodeNames().size(); ++i)
-        {
-            VirtualRobot::RobotNodePtr rnbody = rnsLeftBody->getNode(i);
-            VirtualRobot::RobotNodePtr rnjoint = leftRNS->getNode(i + 1);
-
-            Eigen::MatrixXf jacobipos_rn = 0.001 * leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
-            Eigen::MatrixXf jacobiori_rn = leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
-
-
-            float m = rnbody->getMass();
-            Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
-            leftInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
-        }
-
-        for (size_t i = 0; i < rnsRightBody->getNodeNames().size(); ++i)
-        {
-            VirtualRobot::RobotNodePtr rnbody = rnsRightBody->getNode(i);
-            VirtualRobot::RobotNodePtr rnjoint = rightRNS->getNode(i + 1);
-
-            Eigen::MatrixXf jacobipos_rn = 0.001 * rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
-            Eigen::MatrixXf jacobiori_rn = rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
-
-
-            float m = rnbody->getMass();
-            Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
-
-            rightInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
-        }
-        Eigen::MatrixXf M = Eigen::MatrixXf::Zero(n, n);
-        M.topLeftCorner(nl, nl) = leftInertialMatrix;
-        M.bottomRightCorner(nr, nr) = rightInertialMatrix;
-
-        Eigen::MatrixXf Mc = M + projection * M - M * projection;
-
-
-        // force filter and measure
-        double filterFactor = 1 / ((filterTimeConstant / deltaT) + 1);
-
-        filtered_leftForce = (1 - filterFactor) * filtered_leftForce + filterFactor * (leftForceTorque->force - offset_leftForce);
-        filtered_leftTorque = (1 - filterFactor) * filtered_leftTorque + filterFactor * (leftForceTorque->torque - offset_leftTorque);
-        filtered_rightForce = (1 - filterFactor) * filtered_rightForce + filterFactor * (rightForceTorque->force - offset_rightForce);
-        filtered_rightTorque = (1 - filterFactor) * filtered_rightForce + filterFactor * (rightForceTorque->torque - offset_rightTorque);
-
-        Eigen::VectorXf filteredForce;
-        filteredForce.resize(12);
-        filteredForce << filtered_leftForce, filtered_leftTorque, filtered_rightForce, filtered_rightTorque;
-        filteredForce.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(0, 0);
-        filteredForce.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(3, 0);
-        filteredForce.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(6, 0);
-        filteredForce.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(9, 0);
-
-
-        // calculate force control part
-        Eigen::VectorXf constrainedForceMeasure = proj2GraspNullspace  * filteredForce;
-
-        constrainedForceMeasure.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(0, 0);
-        constrainedForceMeasure.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(3, 0);
-        constrainedForceMeasure.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(6, 0);
-        constrainedForceMeasure.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(9, 0);
-        Eigen::VectorXf forceConstrained = Eigen::VectorXf::Zero(12);
-        for (size_t i = 0; i < 12; i++)
-        {
-            ftPIDController[i]->update(constrainedForceMeasure(i), forceC_des(i));
-            forceConstrained(i) = ftPIDController[i]->getControlValue() + forceC_des(i);
-            //            forceConstrained(i) = forceC_des(i) + kpf(i) * (forceC_des(i) - filtered_leftForce(i)); // TODO: add PID controller
-            //            forceConstrained(i + 3) = forceC_des(i + 3) + kpf(i + 3) * (forceC_des(i + 3) - filtered_leftTorque(i)); // TODO: add PID controller
-            //            forceConstrained(i + 6) = forceC_des(i + 6) + kpf(i + 6) * (forceC_des(i + 6) - filtered_rightForce(i)); // TODO: add PID controller
-            //            forceConstrained(i + 9) = forceC_des(i + 9) + kpf(i + 9) * (forceC_des(i + 9) - filtered_rightTorque(i)); // TODO: add PID controller
-        }
-        forceConstrained.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(0, 0);
-        forceConstrained.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(3, 0);
-        forceConstrained.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(6, 0);
-        forceConstrained.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(9, 0);
-
-
-
-
-
-
-        // unconstrained space controller
-        Eigen::Vector6f leftJointControlWrench;
-        {
-            Eigen::Vector3f targetTCPLinearVelocity;
-            targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1), 0.001 * leftTargetVel(2);
-
-            Eigen::Vector3f currentTCPLinearVelocity;
-            currentTCPLinearVelocity <<  currentLeftTwist(0),   currentLeftTwist(1),  currentLeftTwist(2);
-            Eigen::Vector3f currentTCPAngularVelocity;
-            currentTCPAngularVelocity << currentLeftTwist(3),   currentLeftTwist(4),  currentLeftTwist(5);
-            Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
-            Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
-
-            Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-            Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
-            Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-            Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
-            leftJointControlWrench <<  tcpDesiredForce, tcpDesiredTorque;
-        }
-
-
-        Eigen::Vector6f rightJointControlWrench;
-        {
-            Eigen::Vector3f targetTCPLinearVelocity;
-            targetTCPLinearVelocity << 0.001 * rightTargetVel(0), 0.001 * rightTargetVel(1), 0.001 * rightTargetVel(2);
-
-            Eigen::Vector3f currentTCPLinearVelocity;
-            currentTCPLinearVelocity <<  currentRightTwist(0),  currentRightTwist(1), currentRightTwist(2);
-            Eigen::Vector3f currentTCPAngularVelocity;
-            currentTCPAngularVelocity << currentRightTwist(3),   currentRightTwist(4),  currentRightTwist(5);
-            Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
-            Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
-
-            Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) - dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-            Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
-            Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-            Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
-            rightJointControlWrench <<   tcpDesiredForce, tcpDesiredTorque;
-        }
-
-        Eigen::VectorXf forceUnconstrained(12);
-        forceUnconstrained << leftJointControlWrench, rightJointControlWrench;
-
-        Eigen::VectorXf unConstrainedForceMeasure = pinvGraspMatrix * graspMatrix * filteredForce;
-
-        Eigen::MatrixXf taskSpaceInertia = (jacobi * M.inverse() *  jacobi.transpose()).inverse();
-
-
-
-        forceUnconstrained = taskSpaceInertia * forceUnconstrained ;//+ unConstrainedForceMeasure) - unConstrainedForceMeasure;
-
-        Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
-        Eigen::VectorXf rightNullspaceTorque = knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
-        float lambda = 2;
-
-        Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
-        Eigen::MatrixXf jtpinvR = leftIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
-        forceUnconstrained.head(8) = forceUnconstrained.head(8) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
-        forceUnconstrained.tail<8>() = forceUnconstrained.tail<8>() + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
-
-        //
-        ////        Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
-        //        Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceUnconstrained.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
-
-        //
-        ////        Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-        //        Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceUnconstrained.tail<6>() + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
-        //        Eigen::VectorXf torqueUnconstrained(16);
-        //        torqueUnconstrained << leftJointDesiredTorques, rightJointDesiredTorques;
-
-        // constrained space control
-
-        //        Eigen::MatrixXf mu = (Eigen::MatrixXf::Identity(n, n) - projection) * M * Mc.inverse();
-        //        Eigen::VectorXf qacc;
-        //        qacc.resize(n);
-        //        for (size_t i = 0; i < leftAccelerationSensors.size(); ++i)
-        //        {
-        //            qacc(i) = 0.001 * leftAccelerationSensors[i]->acceleration;
-        //        }
-
-        //        for (size_t i = 0; i < rightAccelerationSensors.size(); ++i)
-        //        {
-        //            qacc(i + leftAccelerationSensors.size()) = 0.001 * rightAccelerationSensors[i]->acceleration;
-        //        }
-
-
-        //        Eigen::VectorXf torqueConstrained = mu * (torqueUnconstrained + (Eigen::MatrixXf::Identity(n, n) - projection) * qacc) - jacobiConstrained.transpose() * forceConstrained;
-
-
-        // all torques
-
-
-
-        //        Eigen::VectorXf torqueInput = projection * torqueUnconstrained + projection * torqueConstrained;
-        //        Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * (Eigen::MatrixXf::Identity(12,12) - proj2GraspNullspace) * forceConstrained;
-        //        Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * forceConstrained;
-        Eigen::VectorXf torqueInput = jacobi.transpose() * (forceUnconstrained + forceConstrained);
-
-        Eigen::VectorXf leftJointDesiredTorques = torqueInput.head(nl);
-        Eigen::VectorXf rightJointDesiredTorques = torqueInput.block<8, 1>(8, 0);
-
-        // torque limit
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            float desiredTorque = leftJointDesiredTorques(i);
-
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-
-            desiredTorque = (desiredTorque >  torqueLimit) ? torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
-            debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
-
-            leftTargets.at(i)->torque = desiredTorque;
-        }
-
-
-
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            float desiredTorque = rightJointDesiredTorques(i);
-
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-
-            desiredTorque = (desiredTorque >  torqueLimit) ? torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
-            debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
-
-            rightTargets.at(i)->torque = desiredTorque;
-        }
-
-        //        debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
-        //        debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
-        //        debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
-        //        debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
-        //        debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
-        //        debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
-        //        debugDataInfo.getWriteBuffer().quatError = errorAngle;
-        //        debugDataInfo.getWriteBuffer().posiError = posiError;
-        debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
-        debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
-        debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
-        debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
-        debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
-        debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
-
-
-        debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
-        debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
-        debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
-
-        debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
-        debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
-        debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
-
-
-        for (size_t i = 0 ; i < 12; ++i)
-        {
-            std::stringstream ss;
-            ss << i;
-            std::string name = "forceConstrained_" + ss.str();
-            debugDataInfo.getWriteBuffer().constrained_force[name] = filteredForce(i);
-        }
-
-
-        debugDataInfo.commitWrite();
-
-
-
-        //        Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
-        //        float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
-        //        if (normLinearVelocity > cfg->maxLinearVel)
-        //        {
-        //            leftTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
-        //        }
-        //        float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
-        //        if (normAngularVelocity > cfg->maxAngularVel)
-        //        {
-        //            leftTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
-        //        }
-
-        //        Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
-        //        Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
-        //        Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
-
-        //        for (size_t i = 0; i < leftTargets.size(); ++i)
-        //        {
-        //            leftTargets.at(i)->velocity = jnvL(i);
-        //        }
-
-        //        Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
-        //        normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
-        //        if (normLinearVelocity > cfg->maxLinearVel)
-        //        {
-        //            rightTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
-        //        }
-        //        normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
-        //        if (normAngularVelocity > cfg->maxAngularVel)
-        //        {
-        //            rightTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
-        //        }
-        //        Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
-        //        Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
-        //        Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
-
-        //        for (size_t i = 0; i < rightTargets.size(); ++i)
-        //        {
-        //            rightTargets.at(i)->velocity = jnvR(i);
-        //        }
-    }
-
-    void NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        if (name == "LeftLeader")
-        {
-            leftGroup.at(0)->learnDMPFromFiles(fileNames);
-        }
-        else if (name == "LeftFollower")
-        {
-            rightGroup.at(1)->learnDMPFromFiles(fileNames);
-        }
-        else if (name == "RightLeader")
-        {
-            rightGroup.at(0)->learnDMPFromFiles(fileNames);
-        }
-        else
-        {
-            leftGroup.at(1)->learnDMPFromFiles(fileNames);
-        }
-    }
-
-
-    void NJointBimanualCCDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-        if (leaderName == "Left")
-        {
-            leftGroup.at(0)->setViaPose(u, viapoint);
-        }
-        else
-        {
-            rightGroup.at(0)->setViaPose(u, viapoint);
-        }
-    }
-
-    void NJointBimanualCCDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard(controllerMutex);
-        if (leaderName == "Left")
-        {
-            leftGroup.at(0)->setGoalPoseVec(goals);
-        }
-        else
-        {
-            rightGroup.at(0)->setGoalPoseVec(goals);
-        }
-
-    }
-
-    void NJointBimanualCCDMPController::changeLeader(const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-
-        if (leaderName == "Left")
-        {
-            leaderName = "Right";
-            rightGroup.at(0)->canVal = virtualtimer;
-            rightGroup.at(1)->canVal = virtualtimer;
-        }
-        else
-        {
-            leaderName = "Left";
-            leftGroup.at(0)->canVal = virtualtimer;
-            leftGroup.at(1)->canVal = virtualtimer;
-        }
-
-    }
-
-
-
-
-
-    void NJointBimanualCCDMPController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
-    {
-        virtualtimer = cfg->timeDuration;
-
-        Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
-
-        leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
-        rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
-
-
-        ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
-
-        leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
-        rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
-
-        controllerTask->start();
-    }
-
-    Eigen::Matrix4f NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
-    {
-        Eigen::Matrix4f localPose;
-        localPose.block<3, 3>(0, 0) = globalTargetPose.block<3, 3>(0, 0) * newCoordinate.block<3, 3>(0, 0).inverse();
-        localPose.block<3, 1>(0, 3) = localPose.block<3, 3>(0, 0) * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
-
-        return localPose;
-    }
-
-
-    void NJointBimanualCCDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-
-        StringVariantBaseMap datafields;
-        auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
-        for (auto& pair : constrained_force)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-
-        datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
-        datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
-        datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
-        datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
-        datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
-        datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
-
-
-        datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
-        datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
-        datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
-        datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
-        datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
-        datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
-
-        //        StringVariantBaseMap datafields;
-        //        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        //        for (auto& pair : values)
-        //        {
-        //            datafields[pair.first] = new Variant(pair.second);
-        //        }
-
-        //        auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
-        //        for (auto& pair : dmpTargets)
-        //        {
-        //            datafields[pair.first] = new Variant(pair.second);
-        //        }
-
-        //        auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
-        //        for (auto& pair : currentPose)
-        //        {
-        //            datafields[pair.first] = new Variant(pair.second);
-        //        }
-
-        //        datafields["leaderCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderCanVal);
-        //        datafields["leadermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadermpcFactor);
-        //        datafields["leaderError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadererror);
-        //        datafields["leaderposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderposError);
-        //        datafields["leaderoriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderoriError);
-
-        //        datafields["followerCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerCanVal);
-        //        datafields["followermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().followermpcFactor);
-        //        datafields["followerError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followererror);
-        //        datafields["followerposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerposError);
-        //        datafields["followeroriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followeroriError);
-
-        debugObs->setDebugChannel("DMPController", datafields);
-    }
-
-    void NJointBimanualCCDMPController::onInitComponent()
-    {
-        ARMARX_INFO << "init ...";
-        controllerTask = new PeriodicTask<NJointBimanualCCDMPController>(this, &NJointBimanualCCDMPController::controllerRun, 0.3);
-    }
-
-    void NJointBimanualCCDMPController::onDisconnectComponent()
-    {
-        controllerTask->stop();
-        ARMARX_INFO << "stopped ...";
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h
deleted file mode 100644
index 7c80df635685a73315b21b0bb966eda79e4c0d15..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h
+++ /dev/null
@@ -1,236 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-namespace armarx
-{
-    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPController);
-    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPControllerControlData);
-
-    using ViaPoint = std::pair<double, DMP::DVec >;
-    using ViaPointsSet = std::vector<ViaPoint >;
-    class NJointBimanualCCDMPControllerControlData
-    {
-    public:
-        Eigen::VectorXf leftTargetVel;
-        Eigen::VectorXf rightTargetVel;
-
-        Eigen::Matrix4f leftTargetPose;
-        Eigen::Matrix4f rightTargetPose;
-    };
-
-    class NJointBimanualCCDMPController :
-        public NJointControllerWithTripleBuffer<NJointBimanualCCDMPControllerControlData>,
-        public NJointBimanualCCDMPControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointBimanualCCDMPControllerConfigPtr;
-        NJointBimanualCCDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointBimanualCCDMPControllerInterface interface
-        void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&);
-        bool isFinished(const Ice::Current&)
-        {
-            return false;
-        }
-
-        void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&);
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-
-        void changeLeader(const Ice::Current&);
-
-        double getVirtualTime(const Ice::Current&)
-        {
-            return virtualtimer;
-        }
-
-        std::string getLeaderName(const Ice::Current&)
-        {
-            return leaderName;
-        }
-
-    protected:
-
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitComponent();
-        void onDisconnectComponent();
-        void controllerRun();
-    private:
-
-        Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose);
-
-        Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
-        Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
-        {
-            Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
-            newCoordinate(0, 3) = newCoordinateVec.at(0);
-            newCoordinate(1, 3) = newCoordinateVec.at(1);
-            newCoordinate(2, 3) = newCoordinateVec.at(2);
-
-            Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
-            globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
-            globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
-            globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
-
-            return getLocalPose(newCoordinate, globalTargetPose);
-
-        }
-
-        struct DebugBufferData
-        {
-            StringFloatDictionary desired_torques;
-            StringFloatDictionary constrained_force;
-            float leftTargetPose_x;
-            float leftTargetPose_y;
-            float leftTargetPose_z;
-            float rightTargetPose_x;
-            float rightTargetPose_y;
-            float rightTargetPose_z;
-
-            float leftCurrentPose_x;
-            float leftCurrentPose_y;
-            float leftCurrentPose_z;
-            float rightCurrentPose_x;
-            float rightCurrentPose_y;
-            float rightCurrentPose_z;
-
-            //            StringFloatDictionary latestTargetVelocities;
-            //            StringFloatDictionary dmpTargets;
-            //            StringFloatDictionary currentPose;
-
-            //            double leadermpcFactor;
-            //            double leadererror;
-            //            double leaderposError;
-            //            double leaderoriError;
-            //            double leaderCanVal;
-
-            //            double followermpcFactor;
-            //            double followererror;
-            //            double followerposError;
-            //            double followeroriError;
-            //            double followerCanVal;
-
-
-        };
-
-        TripleBuffer<DebugBufferData> debugDataInfo;
-
-        struct NJointBimanualCCDMPControllerSensorData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentLeftPose;
-            Eigen::Matrix4f currentRightPose;
-            Eigen::VectorXf currentLeftTwist;
-            Eigen::VectorXf currentRightTwist;
-
-
-        };
-        TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
-
-
-        std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
-        std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
-        std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
-        const SensorValueForceTorque* rightForceTorque;
-        const SensorValueForceTorque* leftForceTorque;
-
-        NJointBimanualCCDMPControllerConfigPtr cfg;
-        VirtualRobot::DifferentialIKPtr leftIK;
-        VirtualRobot::DifferentialIKPtr rightIK;
-
-        std::vector<TaskSpaceDMPControllerPtr > leftGroup;
-        std::vector<TaskSpaceDMPControllerPtr > rightGroup;
-        std::vector<TaskSpaceDMPControllerPtr > bothLeaderGroup;
-
-
-        std::string leaderName;
-
-        VirtualRobot::RobotNodePtr tcpLeft;
-        VirtualRobot::RobotNodePtr tcpRight;
-
-        double virtualtimer;
-
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointBimanualCCDMPController>::pointer_type controllerTask;
-
-        Eigen::VectorXf leftDesiredJointValues;
-        Eigen::VectorXf rightDesiredJointValues;
-
-        float KoriFollower;
-        float KposFollower;
-        float DposFollower;
-        float DoriFollower;
-
-        Eigen::VectorXf forceC_des;
-        float boxWidth;
-
-        Eigen::Vector3f kpos;
-        Eigen::Vector3f kori;
-        Eigen::Vector3f dpos;
-        Eigen::Vector3f dori;
-        Eigen::VectorXf kpf;
-        Eigen::VectorXf kif;
-
-        float knull;
-        float dnull;
-
-        std::vector<std::string> leftJointNames;
-        std::vector<std::string> rightJointNames;
-
-        float torqueLimit;
-        VirtualRobot::RobotNodeSetPtr leftRNS;
-        VirtualRobot::RobotNodeSetPtr rightRNS;
-        VirtualRobot::RobotNodeSetPtr rnsLeftBody;
-        VirtualRobot::RobotNodeSetPtr rnsRightBody;
-
-        Eigen::Vector3f filtered_leftForce;
-        Eigen::Vector3f filtered_leftTorque;
-        Eigen::Vector3f filtered_rightForce;
-        Eigen::Vector3f filtered_rightTorque;
-        float filterTimeConstant;
-
-        std::vector<PIDControllerPtr> ftPIDController;
-
-        Eigen::Vector3f offset_leftForce;
-        Eigen::Vector3f offset_leftTorque;
-        Eigen::Vector3f offset_rightForce;
-        Eigen::Vector3f offset_rightTorque;
-
-        bool isForceCompensateDone;
-
-        // NJointBimanualCCDMPControllerInterface interface
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
deleted file mode 100644
index 34ee1edba375358ef8fe49a61d1505ee57d16c0b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
+++ /dev/null
@@ -1,496 +0,0 @@
-#include "NJointBimanualForceMPController.h"
-#include <random>
-
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointBimanualForceMPController> registrationControllerNJointBimanualForceMPController("NJointBimanualForceMPController");
-
-    NJointBimanualForceMPController::NJointBimanualForceMPController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        useSynchronizedRtRobot();
-        cfg = NJointBimanualForceMPControllerConfigPtr::dynamicCast(config);
-
-        VirtualRobot::RobotNodeSetPtr lrns = rtGetRobot()->getRobotNodeSet("LeftArm");
-        for (size_t i = 0; i < lrns->getSize(); ++i)
-        {
-            std::string jointName = lrns->getNode(i)->getName();
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocity sensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No position sensor available for " << jointName;
-            }
-
-            leftVelocitySensors.push_back(velocitySensor);
-            leftPositionSensors.push_back(positionSensor);
-        };
-
-        VirtualRobot::RobotNodeSetPtr rrns = rtGetRobot()->getRobotNodeSet("RightArm");
-        for (size_t i = 0; i < rrns->getSize(); ++i)
-        {
-            std::string jointName = rrns->getNode(i)->getName();
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocity sensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No position sensor available for " << jointName;
-            }
-
-            rightVelocitySensors.push_back(velocitySensor);
-            rightPositionSensors.push_back(positionSensor);
-        };
-        const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
-        leftForceTorque = svlf->asA<SensorValueForceTorque>();
-        const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
-        rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
-        leftIK.reset(new VirtualRobot::DifferentialIK(lrns, lrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        rightIK.reset(new VirtualRobot::DifferentialIK(rrns, rrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-        leftTCP = lrns->getTCP();
-        rightTCP = rrns->getTCP();
-
-        leftLearned = false;
-        rightLearned = false;
-
-        // set tcp controller
-        leftTCPController.reset(new CartesianVelocityController(lrns, leftTCP));
-        rightTCPController.reset(new CartesianVelocityController(rrns, rightTCP));
-
-
-        finished = false;
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpStyle;
-        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-        leftDMPController.reset(new TaskSpaceDMPController("Left", taskSpaceDMPConfig, false));
-        rightDMPController.reset(new TaskSpaceDMPController("Right", taskSpaceDMPConfig, false));
-
-
-        // initialize tcp position and orientation
-        NJointBimanualForceMPControllerSensorData initSensorData;
-        initSensorData.leftPoseInRootFrame = leftTCP->getPoseInRootFrame();
-        initSensorData.leftTwistInRootFrame.setZero();
-        initSensorData.rightPoseInRootFrame = rightTCP->getPoseInRootFrame();
-        initSensorData.rightTwistInRootFrame.setZero();
-        initSensorData.leftForceInRootFrame.setZero();
-        initSensorData.rightForceInRootFrame.setZero();
-        controllerSensorData.reinitAllBuffers(initSensorData);
-
-        NJointBimanualForceMPControllerControlData initData;
-        initData.leftTargetPose = leftTCP->getPoseInRootFrame();
-        initData.rightTargetPose = rightTCP->getPoseInRootFrame();
-        reinitTripleBuffer(initData);
-
-
-        Kp_LinearVel = cfg->Kp_LinearVel;
-        Kp_AngularVel = cfg->Kp_AngularVel;
-        Kd_LinearVel = cfg->Kd_LinearVel;
-        Kd_AngularVel = cfg->Kd_AngularVel;
-
-        forceIterm = 0;
-        I_decay = 0.9;
-        xvel = 0;
-
-        leftFilteredValue.setZero();
-        rightFilteredValue.setZero();
-
-        for (size_t i = 0; i < 3; ++i)
-        {
-            leftForceOffset(i) = cfg->leftForceOffset.at(i);
-        }
-        for (size_t i = 0; i < 3; ++i)
-        {
-            rightForceOffset(i) = cfg->rightForceOffset.at(i);
-        }
-
-
-        targetSupportForce = cfg->targetSupportForce;
-
-        NJointBimanualForceMPControllerInterfaceData initInterfaceData;
-        initInterfaceData.leftTcpPoseInRootFrame = leftTCP->getPoseInRootFrame();
-        initInterfaceData.rightTcpPoseInRootFrame = rightTCP->getPoseInRootFrame();
-        interfaceData.reinitAllBuffers(initInterfaceData);
-    }
-
-    std::string NJointBimanualForceMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointBimanualForceMPController";
-    }
-
-    void NJointBimanualForceMPController::controllerRun()
-    {
-        if (!controllerSensorData.updateReadBuffer() || !leftDMPController || !rightDMPController)
-        {
-            return;
-        }
-        double deltaT = controllerSensorData.getReadBuffer().deltaT;
-        Eigen::Matrix4f leftPose = controllerSensorData.getReadBuffer().leftPoseInRootFrame;
-        Eigen::Matrix4f rightPose = controllerSensorData.getReadBuffer().rightPoseInRootFrame;
-        Eigen::Vector6f leftTwist = controllerSensorData.getReadBuffer().leftTwistInRootFrame;
-        Eigen::Vector6f rightTwist = controllerSensorData.getReadBuffer().rightTwistInRootFrame;
-        Eigen::Vector3f leftForce = controllerSensorData.getReadBuffer().leftForceInRootFrame;
-        Eigen::Vector3f rightForce = controllerSensorData.getReadBuffer().rightForceInRootFrame;
-
-        float forceOnHands = (leftForce + rightForce)(2) ;
-
-        xvel = cfg->forceP * (targetSupportForce + forceOnHands) + I_decay * cfg->forceI * forceIterm;
-
-        forceIterm += targetSupportForce - forceOnHands;
-
-        canVal = canVal + forceSign * xvel * deltaT;
-        //        canVal = cfg->timeDuration + forceSign * xvel;
-
-
-        if (canVal > cfg->timeDuration)
-        {
-            canVal = cfg->timeDuration;
-        }
-
-
-        leftDMPController->canVal = canVal;
-        rightDMPController->canVal = canVal;
-
-        leftDMPController->flow(deltaT, leftPose, leftTwist);
-        rightDMPController->flow(deltaT, rightPose, rightTwist);
-
-        if (canVal < 1e-8)
-        {
-            finished = true;
-        }
-
-
-        std::vector<double> leftTargetState = leftDMPController->getTargetPose();
-        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_x"] = leftTargetState[0];
-        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_y"] = leftTargetState[1];
-        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_z"] = leftTargetState[2];
-        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qw"] = leftTargetState[3];
-        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qx"] = leftTargetState[4];
-        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qy"] = leftTargetState[5];
-        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qz"] = leftTargetState[6];
-        std::vector<double> rightTargetState = rightDMPController->getTargetPose();
-        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_x"] = rightTargetState[0];
-        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_y"] = rightTargetState[1];
-        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_z"] = rightTargetState[2];
-        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qw"] = rightTargetState[3];
-        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qx"] = rightTargetState[4];
-        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qy"] = rightTargetState[5];
-        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qz"] = rightTargetState[6];
-
-
-        {
-            debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = leftPose(0, 3);
-            debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = leftPose(1, 3);
-            debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = leftPose(2, 3);
-            VirtualRobot::MathTools::Quaternion leftQuat = VirtualRobot::MathTools::eigen4f2quat(leftPose);
-            debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = leftQuat.w;
-            debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = leftQuat.x;
-            debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = leftQuat.y;
-            debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = leftQuat.z;
-        }
-        {
-            debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = rightPose(0, 3);
-            debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = rightPose(1, 3);
-            debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = rightPose(2, 3);
-            VirtualRobot::MathTools::Quaternion rightQuat = VirtualRobot::MathTools::eigen4f2quat(rightPose);
-            debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = rightQuat.w;
-            debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = rightQuat.x;
-            debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = rightQuat.y;
-            debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = rightQuat.z;
-        }
-
-        debugOutputData.getWriteBuffer().canVal = canVal;
-        debugOutputData.getWriteBuffer().forceOnHands = forceOnHands;
-        debugOutputData.commitWrite();
-
-
-        getWriterControlStruct().leftTargetPose = leftDMPController->getTargetPoseMat();
-        getWriterControlStruct().rightTargetPose = rightDMPController->getTargetPoseMat();
-        writeControlStruct();
-
-
-    }
-
-
-    void NJointBimanualForceMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        Eigen::Matrix4f leftPose = leftTCP->getPoseInRootFrame();
-        Eigen::Matrix4f rightPose = rightTCP->getPoseInRootFrame();
-        Eigen::MatrixXf leftJacobi = leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
-        Eigen::MatrixXf rightJacobi = leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf qvel;
-        qvel.resize(leftVelocitySensors.size());
-        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
-        {
-            qvel(i) = leftVelocitySensors[i]->velocity;
-        }
-        Eigen::Vector6f leftTwist = leftJacobi * qvel;
-        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
-        {
-            qvel(i) = rightVelocitySensors[i]->velocity;
-        }
-        Eigen::Vector6f rightTwist = rightJacobi * qvel;
-
-        leftFilteredValue = (1 - cfg->filterCoeff) * (leftForceTorque->force - leftForceOffset) + cfg->filterCoeff * leftFilteredValue;
-        rightFilteredValue = (1 - cfg->filterCoeff) * (rightForceTorque->force - rightForceOffset) + cfg->filterCoeff * rightFilteredValue;
-
-        Eigen::Matrix4f leftSensorFrame = rtGetRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
-        Eigen::Matrix4f rightSensorFrame = rtGetRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
-
-        Eigen::Vector3f leftForceInRootFrame = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0) * leftFilteredValue;
-        Eigen::Vector3f rightForceInRootFrame = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0) * rightFilteredValue;
-
-        controllerSensorData.getWriteBuffer().leftPoseInRootFrame = leftPose;
-        controllerSensorData.getWriteBuffer().rightPoseInRootFrame = rightPose;
-        controllerSensorData.getWriteBuffer().leftTwistInRootFrame = leftTwist;
-        controllerSensorData.getWriteBuffer().rightTwistInRootFrame = rightTwist;
-        controllerSensorData.getWriteBuffer().leftForceInRootFrame = leftForceInRootFrame;
-        controllerSensorData.getWriteBuffer().rightForceInRootFrame = rightForceInRootFrame;
-        controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
-        controllerSensorData.commitWrite();
-
-        interfaceData.getWriteBuffer().leftTcpPoseInRootFrame = leftPose;
-        interfaceData.getWriteBuffer().rightTcpPoseInRootFrame = rightPose;
-        interfaceData.commitWrite();
-
-
-
-
-        Eigen::Matrix4f targetPose = rtGetControlStruct().leftTargetPose;
-        Eigen::Matrix4f currentPose = leftPose;
-        Eigen::Vector6f leftVel;
-
-        {
-            Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
-            Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-            Eigen::Vector6f rtTargetVel;
-            rtTargetVel.block<3, 1>(0, 0) = Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3))
-                                            + Kd_LinearVel * (- leftTwist.block<3, 1>(0, 0));
-            rtTargetVel.block<3, 1>(3, 0) = Kp_AngularVel * errorRPY + Kd_AngularVel * (- leftTwist.block<3, 1>(3, 0));
-
-            float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
-            if (normLinearVelocity > cfg->maxLinearVel)
-            {
-                rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
-            }
-
-            float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
-            if (normAngularVelocity > cfg->maxAngularVel)
-            {
-                rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
-            }
-
-            for (size_t i = 0; i < 6; i++)
-            {
-                leftVel(i) = rtTargetVel(i);
-            }
-
-        }
-
-        // cartesian vel controller
-
-
-        targetPose = rtGetControlStruct().rightTargetPose;
-        currentPose = rightPose;
-        Eigen::Vector6f rightVel;
-
-        {
-            Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
-            Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-            Eigen::Vector6f rtTargetVel;
-            rtTargetVel.block<3, 1>(0, 0) = Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3))
-                                            + Kd_LinearVel * (- rightTwist.block<3, 1>(0, 0));
-            rtTargetVel.block<3, 1>(3, 0) = Kp_AngularVel * errorRPY + Kd_AngularVel * (- rightTwist.block<3, 1>(3, 0));
-
-            float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
-            if (normLinearVelocity > cfg->maxLinearVel)
-            {
-                rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
-            }
-
-            float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
-            if (normAngularVelocity > cfg->maxAngularVel)
-            {
-                rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
-            }
-
-            for (size_t i = 0; i < 6; i++)
-            {
-                rightVel(i) = rtTargetVel(i);
-            }
-        }
-
-
-        Eigen::VectorXf leftJTV = leftTCPController->calculate(leftVel, cfg->KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection::All);
-        for (size_t i = 0; i < leftTargets.size(); ++i)
-        {
-            leftTargets.at(i)->velocity = leftJTV(i);
-            if (!leftTargets.at(i)->isValid())
-            {
-                ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
-                                 << "Velocity controller target is invalid - setting to zero! set value: " << leftTargets.at(i)->velocity;
-                leftTargets.at(i)->velocity = 0.0f;
-            }
-        }
-
-        Eigen::VectorXf rightJTV = rightTCPController->calculate(rightVel, cfg->KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection::All);
-        for (size_t i = 0; i < rightTargets.size(); ++i)
-        {
-            rightTargets.at(i)->velocity = rightJTV(i);
-            if (!rightTargets.at(i)->isValid())
-            {
-                ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
-                                 << "Velocity controller target is invalid - setting to zero! set value: " << rightTargets.at(i)->velocity;
-                rightTargets.at(i)->velocity = 0.0f;
-            }
-        }
-
-
-    }
-
-
-    void NJointBimanualForceMPController::learnDMPFromFiles(const std::string& whichDMP, const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        ARMARX_IMPORTANT << "Learn DMP " << whichDMP;
-        if (whichDMP == "Left")
-        {
-            leftDMPController->learnDMPFromFiles(fileNames);
-            leftLearned = true;
-            ARMARX_INFO << "Left Learned";
-        }
-        if (whichDMP == "Right")
-        {
-            rightDMPController->learnDMPFromFiles(fileNames);
-            rightLearned = true;
-            ARMARX_INFO << "Right Learned";
-        }
-
-    }
-
-
-    void NJointBimanualForceMPController::runDMP(const Ice::DoubleSeq&  leftGoals, const Ice::DoubleSeq&  rightGoals, const Ice::Current&)
-    {
-        if (!leftLearned)
-        {
-            ARMARX_ERROR << "Left DMP is not learned, aborted";
-            return;
-        }
-
-        if (!rightLearned)
-        {
-            ARMARX_ERROR << "Right DMP is not learned, aborted";
-            return;
-        }
-
-        while (!interfaceData.updateReadBuffer())
-        {
-            usleep(100);
-        }
-
-        Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().leftTcpPoseInRootFrame;
-        Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().rightTcpPoseInRootFrame;
-
-        forceSign = leftGoals.at(2) - leftPose(2, 3) > 0 ? -1 : 1;
-        //        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
-        ARMARX_IMPORTANT << "leftGoals dim: " << leftGoals.size();
-        ARMARX_IMPORTANT << "rightGoals dim: " << rightGoals.size();
-
-        leftDMPController->prepareExecution(leftDMPController->eigen4f2vec(leftPose), leftGoals);
-        rightDMPController->prepareExecution(rightDMPController->eigen4f2vec(rightPose), rightGoals);
-
-        canVal = cfg->timeDuration;
-        finished = false;
-
-        ARMARX_INFO << "run DMP";
-        controllerTask->start();
-
-    }
-
-    void NJointBimanualForceMPController::setViaPoints(const std::string& whichDMP, double u, const Ice::DoubleSeq& viaPoint, const Ice::Current&)
-    {
-        if (whichDMP == "Left")
-        {
-            leftDMPController->setViaPose(u, viaPoint);
-        }
-        if (whichDMP == "Right")
-        {
-            rightDMPController->setViaPose(u, viaPoint);
-        }
-    }
-
-    void NJointBimanualForceMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-        std::string debugName = cfg->debugName;
-        std::string datafieldName = debugName;
-        StringVariantBaseMap datafields;
-
-        auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
-        for (auto& pair : dmpTargets)
-        {
-            datafieldName = pair.first  + "_" + debugName;
-            datafields[datafieldName] = new Variant(pair.second);
-        }
-
-        auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
-        for (auto& pair : currentPose)
-        {
-            datafieldName = pair.first + "_" + debugName;
-            datafields[datafieldName] = new Variant(pair.second);
-        }
-
-        datafieldName = "canVal_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().canVal);
-
-        datafieldName = "forceOnHands_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().forceOnHands);
-        datafieldName = "DMPController_" + debugName;
-
-        debugObs->setDebugChannel(datafieldName, datafields);
-    }
-
-    void NJointBimanualForceMPController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        controllerTask = new PeriodicTask<NJointBimanualForceMPController>(this, &NJointBimanualForceMPController::controllerRun, 0.3);
-    }
-
-    void NJointBimanualForceMPController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-        controllerTask->stop();
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
deleted file mode 100644
index 96e0b7649f2bd776c5c59f5f0a754c1da09f1d42..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
+++ /dev/null
@@ -1,172 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-namespace armarx
-{
-    TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPController);
-    TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPControllerControlData);
-
-    using ViaPoint = std::pair<double, DMP::DVec >;
-    using ViaPointsSet = std::vector<ViaPoint >;
-    class NJointBimanualForceMPControllerControlData
-    {
-    public:
-        Eigen::Matrix4f leftTargetPose;
-        Eigen::Matrix4f rightTargetPose;
-    };
-
-    /**
-     * @brief The NJointBimanualForceMPController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointBimanualForceMPController :
-        public NJointControllerWithTripleBuffer<NJointBimanualForceMPControllerControlData>,
-        public NJointBimanualForceMPControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointBimanualForceMPControllerConfigPtr;
-        NJointBimanualForceMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const override;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        // NJointBimanualForceMPControllerInterface interface
-        void learnDMPFromFiles(const std::string& whichMP, const Ice::StringSeq& fileNames, const Ice::Current&) override;
-        bool isFinished(const Ice::Current&) override
-        {
-            return finished;
-        }
-
-        void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&) override;
-
-        double getCanVal(const Ice::Current&) override
-        {
-            return canVal;
-        }
-
-        void setViaPoints(const std::string& whichDMP, double canVal, const Ice::DoubleSeq& viaPoint, const Ice::Current&) override;
-
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
-        void onInitNJointController() override;
-        void onDisconnectNJointController() override;
-
-        void controllerRun();
-    private:
-        struct DebugBufferData
-        {
-            StringFloatDictionary dmpTargets;
-            StringFloatDictionary currentPose;
-            double canVal;
-            float forceOnHands;
-        };
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-        struct NJointBimanualForceMPControllerSensorData
-        {
-            Eigen::Matrix4f leftPoseInRootFrame;
-            Eigen::Vector6f leftTwistInRootFrame;
-
-            Eigen::Matrix4f rightPoseInRootFrame;
-            Eigen::Vector6f rightTwistInRootFrame;
-
-            Eigen::Vector3f leftForceInRootFrame;
-            Eigen::Vector3f rightForceInRootFrame;
-            double deltaT;
-        };
-        TripleBuffer<NJointBimanualForceMPControllerSensorData> controllerSensorData;
-
-        struct NJointBimanualForceMPControllerInterfaceData
-        {
-            Eigen::Matrix4f leftTcpPoseInRootFrame;
-            Eigen::Matrix4f rightTcpPoseInRootFrame;
-        };
-        TripleBuffer<NJointBimanualForceMPControllerInterfaceData> interfaceData;
-
-        std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
-        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
-        std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
-        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
-        const SensorValueForceTorque* rightForceTorque;
-        const SensorValueForceTorque* leftForceTorque;
-
-        VirtualRobot::DifferentialIKPtr leftIK;
-        VirtualRobot::DifferentialIKPtr rightIK;
-
-
-        TaskSpaceDMPControllerPtr leftDMPController;
-        TaskSpaceDMPControllerPtr rightDMPController;
-
-        // velocity ik controller parameters
-        CartesianVelocityControllerPtr leftTCPController;
-        CartesianVelocityControllerPtr rightTCPController;
-
-        // dmp parameters
-        bool finished;
-        VirtualRobot::RobotNodePtr leftTCP;
-        VirtualRobot::RobotNodePtr rightTCP;
-
-
-        Eigen::VectorXf targetVels;
-        Eigen::Matrix4f targetPose;
-
-        NJointBimanualForceMPControllerConfigPtr cfg;
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointBimanualForceMPController>::pointer_type controllerTask;
-
-        // velocity control
-        float Kp_LinearVel;
-        float Kd_LinearVel;
-        float Kp_AngularVel;
-        float Kd_AngularVel;
-
-        // force control
-        float Kp_f;
-        float Kd_f;
-        float Ki_f;
-
-        double canVal;
-        double xvel;
-
-        float forceIterm;
-        bool leftLearned;
-        bool rightLearned;
-
-        Eigen::Vector3f leftFilteredValue;
-        Eigen::Vector3f rightFilteredValue;
-
-        Eigen::Vector3f leftForceOffset;
-        Eigen::Vector3f rightForceOffset;
-        float targetSupportForce;
-
-        double I_decay;
-
-        float forceSign;
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
deleted file mode 100644
index 98e91a6fe769b1c81cfbd313bf73a5ee8fc7a0d0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+++ /dev/null
@@ -1,595 +0,0 @@
-#include "NJointCCDMPController.h"
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointCCDMPController> registrationControllerNJointCCDMPController("NJointCCDMPController");
-
-    NJointCCDMPController::NJointCCDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        useSynchronizedRtRobot();
-        cfg = NJointCCDMPControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_NOT_NULL(cfg);
-        ARMARX_CHECK_GREATER_EQUAL(cfg->dmpNum, 0);
-        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
-        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
-        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-        for (size_t i = 0; i < rns->getSize(); ++i)
-        {
-            std::string jointName = rns->getNode(i)->getName();
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::VelocityTorque);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-
-            const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
-            const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
-            if (!torqueSensor)
-            {
-                ARMARX_WARNING << "No Torque sensor available for " << jointName;
-            }
-            if (!gravityTorqueSensor)
-            {
-                ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
-            }
-
-            torqueSensors.push_back(torqueSensor);
-            gravityTorqueSensors.push_back(gravityTorqueSensor);
-        };
-
-        tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
-        ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName;
-
-        // set tcp controller
-        tcpController.reset(new CartesianVelocityController(rns, tcp));
-        nodeSetName = cfg->nodeSetName;
-        torquePIDs.resize(tcpController->rns->getSize(), pidController());
-
-
-        ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-        // set DMP
-        isDisturbance = false;
-
-        dmpPtrList.resize(cfg->dmpNum);
-        canVals.resize(cfg->dmpNum);
-        timeDurations = cfg->timeDurations;
-        dmpTypes = cfg->dmpTypes;
-        for (size_t i = 0; i < dmpPtrList.size(); ++i)
-        {
-            dmpPtrList[i].reset(new DMP::UMITSMP(cfg->kernelSize, 2, cfg->tau));
-            canVals[i] = timeDurations[i];
-        }
-        finished = false;
-
-        phaseL = cfg->phaseL;
-        phaseK = cfg->phaseK;
-        phaseDist0 = cfg->phaseDist0;
-        phaseDist1 = cfg->phaseDist1;
-        phaseKpPos = cfg->phaseKpPos;
-        phaseKpOri = cfg->phaseKpOri;
-
-        posToOriRatio = cfg->posToOriRatio;
-        tau = cfg->tau;
-
-        // initialize tcp position and orientation
-        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
-        tcpPosition(0) = pose(0, 3);
-        tcpPosition(1) = pose(1, 3);
-        tcpPosition(2) = pose(2, 3);
-        VirtualRobot::MathTools::Quaternion tcpQ = VirtualRobot::MathTools::eigen4f2quat(pose);
-        tcpOrientation.w() = tcpQ.w;
-        tcpOrientation.x() = tcpQ.x;
-        tcpOrientation.y() = tcpQ.y;
-        tcpOrientation.z() = tcpQ.z;
-
-        NJointCCDMPControllerSensorData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentPose = pose;
-        controllerSensorData.reinitAllBuffers(initSensorData);
-
-
-        currentStates.resize(cfg->dmpNum);
-        targetSubStates.resize(cfg->dmpNum);
-        targetState.resize(7);
-
-        targetVels.resize(6);
-        targetVels.setZero();
-        NJointCCDMPControllerControlData initData;
-        initData.targetTSVel.resize(6);
-        initData.targetTSVel.setZero();
-        initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
-        initData.torqueKp.resize(tcpController->rns->getSize(), 0);
-        initData.torqueKd.resize(tcpController->rns->getSize(), 0);
-        initData.mode = ModeFromIce(cfg->mode);
-        reinitTripleBuffer(initData);
-
-        learnedDMP.clear();
-
-        amplitudes = cfg->amplitudes;
-    }
-
-    std::string NJointCCDMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointCCDMPController";
-    }
-
-    void NJointCCDMPController::controllerRun()
-    {
-        if (!controllerSensorData.updateReadBuffer())
-        {
-            return;
-        }
-
-        double deltaT = controllerSensorData.getReadBuffer().deltaT;
-
-        Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
-        Eigen::Vector3f currentPosition;
-        currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
-        double posError = 0;
-        for (size_t i = 0; i < 3; ++i)
-        {
-            posError += pow(currentPosition(i) - targetState[i], 2);
-        }
-        posError = sqrt(posError);
-
-
-        VirtualRobot::MathTools::Quaternion cQuat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
-        Eigen::Quaterniond currentQ;
-        Eigen::Quaterniond targetQ;
-        currentQ.w() = cQuat.w;
-        currentQ.x() = cQuat.x;
-        currentQ.y() = cQuat.y;
-        currentQ.z() = cQuat.z;
-        targetQ.w() = targetState[3];
-        targetQ.x() = targetState[4];
-        targetQ.y() = targetState[5];
-        targetQ.z() = targetState[6];
-
-        double oriError = targetQ.angularDistance(currentQ);
-        if (oriError > M_PI)
-        {
-            oriError = 2 * M_PI - oriError;
-        }
-
-        double error = posError + posToOriRatio * oriError;
-
-        double phaseDist;
-        if (isDisturbance)
-        {
-            phaseDist = phaseDist1;
-        }
-        else
-        {
-            phaseDist = phaseDist0;
-        }
-
-        double phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
-        double mpcFactor = 1 - (phaseStop / phaseL);
-
-
-        if (mpcFactor < 0.1)
-        {
-            isDisturbance = true;
-        }
-
-        if (mpcFactor > 0.9)
-        {
-            isDisturbance = false;
-        }
-
-        // run DMP one after another
-        Eigen::Matrix4f targetPose = Eigen::Matrix4f::Identity();
-
-        Eigen::VectorXf dmpVels;
-        dmpVels.resize(6);
-        dmpVels.setZero();
-        for (size_t i = 0; i < static_cast<size_t>(cfg->dmpNum); ++i)
-        {
-            double timeDuration = timeDurations[i];
-            std::string dmpType = dmpTypes[i];
-
-            //double amplitude = 1.0;
-            if (dmpType == "Periodic")
-            {
-                if (canVals[i] <= 1e-8)
-                {
-                    canVals[i] = timeDuration;
-                }
-                //amplitude = amplitudes[i];
-            }
-
-            if (canVals[i] > 1e-8)
-            {
-                DMP::UMITSMPPtr dmpPtr = dmpPtrList[i];
-                double dmpDeltaT = deltaT / timeDuration;
-                double tau = dmpPtr->getTemporalFactor();
-                canVals[i] -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
-
-
-                currentStates[i] = dmpPtr->calculateDirectlyVelocity
-                                   (currentStates[i], canVals[i] / timeDurations[i], dmpDeltaT, targetSubStates[i]);
-
-
-
-                for (size_t j = 0; j < 3; ++j)
-                {
-                    dmpVels(j) += currentStates[i][j].vel / timeDurations[i];
-                }
-
-                Eigen::Quaterniond quatVel0;
-                quatVel0.w() = currentStates[i][3].vel;
-                quatVel0.x() = currentStates[i][4].vel;
-                quatVel0.y() = currentStates[i][5].vel;
-                quatVel0.z() = currentStates[i][6].vel;
-                Eigen::Quaterniond dmpQ;
-                dmpQ.w() = currentStates[i][3].pos;
-                dmpQ.x() = currentStates[i][4].pos;
-                dmpQ.y() = currentStates[i][5].pos;
-                dmpQ.z() = currentStates[i][6].pos;
-                //Eigen::Quaterniond angularVel0 = 2.0 * quatVel0 * dmpQ.inverse();
-                const Eigen::Quaterniond aVtmp0 = quatVel0 * dmpQ.inverse();
-                const Eigen::Quaterniond angularVel0{2 * aVtmp0.w(), 2 * aVtmp0.x(), 2 * aVtmp0.y(), 2 * aVtmp0.z()};
-
-
-                Eigen::Vector3f angularVelVec;
-                angularVelVec << angularVel0.x() / timeDurations[i],
-                              angularVel0.y() / timeDurations[i],
-                              angularVel0.z() / timeDurations[i];
-
-                angularVelVec = targetPose.block<3, 3>(0, 0) * angularVelVec;
-
-                ARMARX_INFO << "i: " << i << " angularVelVec: " << angularVelVec;
-                dmpVels(3) += angularVelVec(0);
-                dmpVels(4) += angularVelVec(1);
-                dmpVels(5) += angularVelVec(2);
-
-                finished = false;
-
-            }
-
-
-
-            Eigen::Matrix4f targetSubMat = VirtualRobot::MathTools::quat2eigen4f(targetSubStates[i][4], targetSubStates[i][5], targetSubStates[i][6], targetSubStates[i][3]);
-            targetSubMat(0, 3) = targetSubStates[i][0];
-            targetSubMat(1, 3) = targetSubStates[i][1];
-            targetSubMat(2, 3) = targetSubStates[i][2];
-
-            targetPose = targetPose * targetSubMat;
-
-        }
-
-        //        ARMARX_INFO << "targetPose: " << targetPose;
-        for (size_t i = 0; i < 3; i++)
-        {
-            double vel0 = dmpVels(i);
-            double vel1 = phaseKpPos * (targetState[i] - currentPose(i, 3));
-            targetVels(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
-        }
-
-
-        Eigen::Quaterniond diffQ = targetQ * currentQ.inverse();
-        const Eigen::Quaterniond quatVel1
-        {
-            phaseKpOri * diffQ.w(), phaseKpOri * diffQ.x(),
-            phaseKpOri * diffQ.y(), phaseKpOri * diffQ.z()
-        };
-        //Eigen::Quaterniond angularVel1 = 2.0 * quatVel1 * currentQ.inverse();
-        const Eigen::Quaterniond aVtmp1 = quatVel1 * currentQ.inverse();
-        const Eigen::Quaterniond angularVel1{2 * aVtmp1.w(), 2 * aVtmp1.x(), 2 * aVtmp1.y(), 2 * aVtmp1.z()};
-        targetVels(3) = mpcFactor * dmpVels(3) + (1 - mpcFactor) * angularVel1.x();
-        targetVels(4) = mpcFactor * dmpVels(4) + (1 - mpcFactor) * angularVel1.y();
-        targetVels(5) = mpcFactor * dmpVels(5) + (1 - mpcFactor) * angularVel1.z();
-
-
-
-        VirtualRobot::MathTools::Quaternion tQuat = VirtualRobot::MathTools::eigen4f2quat(targetPose);
-        targetState[0] = targetPose(0, 3);
-        targetState[1] = targetPose(1, 3);
-        targetState[2] = targetPose(2, 3);
-        targetState[3] = tQuat.w;
-        targetState[4] = tQuat.x;
-        targetState[5] = tQuat.y;
-        targetState[6] = tQuat.z;
-
-
-        debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
-
-        debugOutputData.getWriteBuffer().realTCP["real_x"] = currentPosition[0];
-        debugOutputData.getWriteBuffer().realTCP["real_y"] = currentPosition[1];
-        debugOutputData.getWriteBuffer().realTCP["real_z"] = currentPosition[2];
-        debugOutputData.getWriteBuffer().realTCP["real_qw"] = cQuat.w;
-        debugOutputData.getWriteBuffer().realTCP["real_qx"] = cQuat.x;
-        debugOutputData.getWriteBuffer().realTCP["real_qy"] = cQuat.y;
-        debugOutputData.getWriteBuffer().realTCP["real_qz"] = cQuat.z;
-
-        debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
-        debugOutputData.getWriteBuffer().error = error;
-        debugOutputData.getWriteBuffer().phaseStop = phaseStop;
-        debugOutputData.getWriteBuffer().posError = posError;
-        debugOutputData.getWriteBuffer().oriError = oriError;
-        debugOutputData.getWriteBuffer().deltaT = deltaT;
-
-        debugOutputData.getWriteBuffer().canVal0 = canVals[0];
-
-
-        debugOutputData.commitWrite();
-
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().targetTSVel = targetVels;
-        writeControlStruct();
-    }
-
-
-    void NJointCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-        controllerSensorData.getWriteBuffer().currentPose = tcp->getPoseInRootFrame();
-        controllerSensorData.getWriteBuffer().deltaT = deltaT;
-        controllerSensorData.getWriteBuffer().currentTime += deltaT;
-        controllerSensorData.commitWrite();
-        // cartesian vel controller
-
-        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-
-        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
-
-        Eigen::VectorXf jnv = jtpinv * rtGetControlStruct().targetTSVel;
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            targets.at(i)->velocity = jnv(i);
-        }
-    }
-
-
-    void NJointCCDMPController::learnDMPFromFiles(int dmpId, const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
-        DMP::DVec ratios;
-        DMP::DVec goals;
-        DMP::Vec<DMP::DMPState > starts;
-
-        for (size_t i = 0; i < fileNames.size(); ++i)
-        {
-            DMP::SampledTrajectoryV2 traj;
-            traj.readFromCSVFile(fileNames.at(i));
-            traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
-            trajs.push_back(traj);
-
-            if (i == 0)
-            {
-                goals.resize(traj.dim());
-                starts.resize(traj.dim());
-                for (size_t j = 0; j < goals.size(); ++j)
-                {
-                    goals[j] = traj.rbegin()->getPosition(j);
-                    starts[j].pos = traj.begin()->getPosition(j);
-                    starts[j].vel = traj.begin()->getDeriv(j, 1) * timeDurations[dmpId];
-                }
-            }
-
-            if (i == 0)
-            {
-                ratios.push_back(1.0);
-            }
-            else
-            {
-                ratios.push_back(0.0);
-            }
-
-
-        }
-
-        dmpPtrList[dmpId]->learnFromTrajectories(trajs);
-        dmpPtrList[dmpId]->styleParas = dmpPtrList[dmpId]->getStyleParasWithRatio(ratios);
-
-
-        DMP::Vec<DMP::DMPState > currentState;
-        if (dmpId == 0)
-        {
-            for (size_t i = 0; i < 3; i++)
-            {
-                DMP::DMPState currentPos;
-                currentPos.pos = tcpPosition(i);
-                currentPos.vel = 0;
-                currentState.push_back(currentPos);
-            }
-
-            DMP::DMPState currentPos;
-            currentPos.pos = tcpOrientation.w();
-            currentPos.vel = 0;
-            currentState.push_back(currentPos);
-
-            currentPos.pos = tcpOrientation.x();
-            currentPos.vel = 0;
-            currentState.push_back(currentPos);
-
-            currentPos.pos = tcpOrientation.y();
-            currentPos.vel = 0;
-            currentState.push_back(currentPos);
-
-            currentPos.pos = tcpOrientation.z();
-            currentPos.vel = 0;
-            currentState.push_back(currentPos);
-
-        }
-        else
-        {
-            currentState = starts;
-        }
-        for (size_t i = 0; i < 3; i++)
-        {
-            targetState[i] =  tcpPosition(i);
-        }
-
-        targetState[3] = tcpOrientation.w();
-        targetState[4] = tcpOrientation.x();
-        targetState[5] = tcpOrientation.y();
-        targetState[6] = tcpOrientation.z();
-
-        currentStates[dmpId] = currentState;
-
-        dmpPtrList[dmpId]->prepareExecution(goals, currentState, 1,  tau);
-        dmpPtrList[dmpId]->setTemporalFactor(tau);
-
-        learnedDMP.push_back(dmpId);
-        ARMARX_INFO << "Learned DMP ... ";
-    }
-
-    void NJointCCDMPController::setViaPoints(int dmpId, Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-
-        LockGuardType guard(controllerMutex);
-        dmpPtrList[dmpId]->setViaPoint(u, viapoint);
-    }
-
-    void NJointCCDMPController::setGoals(int dmpId, const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        setViaPoints(dmpId, dmpPtrList[dmpId]->getUMin(), goals, ice);
-    }
-
-    void NJointCCDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
-    {
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
-        getWriterControlStruct().mode = ModeFromIce(mode);
-        writeControlStruct();
-    }
-
-    VirtualRobot::IKSolver::CartesianSelection NJointCCDMPController::ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
-    {
-        if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
-        {
-            return VirtualRobot::IKSolver::CartesianSelection::Position;
-        }
-        if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
-        {
-            return VirtualRobot::IKSolver::CartesianSelection::Orientation;
-        }
-        if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
-        {
-            return VirtualRobot::IKSolver::CartesianSelection::All;
-        }
-        ARMARX_ERROR_S << "invalid mode " << mode;
-        return (VirtualRobot::IKSolver::CartesianSelection)0;
-    }
-
-
-    void NJointCCDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
-    {
-        LockGuardType guard {controlDataMutex};
-        for (size_t i = 0; i < tcpController->rns->getSize(); i++)
-        {
-            getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
-        }
-        writeControlStruct();
-    }
-
-    void NJointCCDMPController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
-    {
-        LockGuardType guard {controlDataMutex};
-        for (size_t i = 0; i < tcpController->rns->getSize(); i++)
-        {
-            getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
-        }
-        writeControlStruct();
-    }
-
-    void NJointCCDMPController::runDMP(const Ice::Current&)
-    {
-
-        const auto dmpNum = static_cast<std::size_t>(cfg->dmpNum);
-        finished = false;
-        if (dmpNum != dmpTypes.size()      ||
-            dmpNum != dmpPtrList.size()    ||
-            dmpNum != learnedDMP.size()    ||
-            dmpNum != canVals.size()       ||
-            dmpNum != currentStates.size() ||
-            dmpNum != targetSubStates.size())
-        {
-            ARMARX_ERROR << "Error: cannot run CCDMP controller. The reason is that some parameters have different sizes";
-            return;
-        }
-        ARMARX_INFO << "run DMP";
-        controllerTask->start();
-
-    }
-
-    void NJointCCDMPController::setTemporalFactor(int dmpId, double tau, const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-        dmpPtrList[dmpId]->setTemporalFactor(tau);
-    }
-
-    void NJointCCDMPController::rtPreActivateController()
-    {
-    }
-
-    void NJointCCDMPController::rtPostDeactivateController()
-    {
-
-    }
-
-    void NJointCCDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
-        for (auto& pair : dmpTargets)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
-        for (auto& pair : realTCP)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-
-        datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
-        datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
-        datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
-        datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
-        datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
-        datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-        datafields["canVal0"] = new Variant(debugOutputData.getUpToDateReadBuffer().canVal0);
-
-        debugObs->setDebugChannel("DMPController", datafields);
-    }
-
-    void NJointCCDMPController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        controllerTask = new PeriodicTask<NJointCCDMPController>(this, &NJointCCDMPController::controllerRun, 0.3);
-    }
-
-    void NJointCCDMPController::onDisconnectNJointController()
-    {
-        controllerTask->stop();
-        ARMARX_INFO << "stopped ...";
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
deleted file mode 100644
index 132e6bda55186b7cdb9b1cb5fa64ef16081897a8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ /dev/null
@@ -1,174 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-
-namespace armarx
-{
-    TYPEDEF_PTRS_HANDLE(NJointCCDMPController);
-    TYPEDEF_PTRS_HANDLE(NJointCCDMPControllerControlData);
-
-    using ViaPoint = std::pair<double, DMP::DVec >;
-    using ViaPointsSet = std::vector<ViaPoint >;
-    class NJointCCDMPControllerControlData
-    {
-    public:
-        Eigen::VectorXf targetTSVel;
-        // cartesian velocity control data
-        std::vector<float> nullspaceJointVelocities;
-        float avoidJointLimitsKp = 0;
-        std::vector<float> torqueKp;
-        std::vector<float> torqueKd;
-        VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
-    };
-
-    /**
-     * @brief The NJointCCDMPController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointCCDMPController :
-        public NJointControllerWithTripleBuffer<NJointCCDMPControllerControlData>,
-        public NJointCCDMPControllerInterface
-    {
-        class pidController
-        {
-        public:
-            float Kp = 0, Kd = 0;
-            float lastError = 0;
-            float update(float dt, float error)
-            {
-                float derivative = (error - lastError) / dt;
-                float retVal = Kp * error + Kd * derivative;
-                lastError = error;
-                return retVal;
-            }
-        };
-    public:
-        using ConfigPtrT = NJointCCDMPControllerConfigPtr;
-        NJointCCDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const override;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        // NJointCCDMPControllerInterface interface
-        void learnDMPFromFiles(int dmpId, const Ice::StringSeq& fileNames, const Ice::Current&) override;
-        bool isFinished(const Ice::Current&) override
-        {
-            return finished;
-        }
-
-        void runDMP(const Ice::Current&) override;
-        void setTemporalFactor(int dmpId, Ice::Double tau, const Ice::Current&) override;
-        void setViaPoints(int dmpId, Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
-        void setGoals(int dmpId, const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
-        void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&) override;
-        void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
-        void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
-    protected:
-        void rtPreActivateController() override;
-        void rtPostDeactivateController() override;
-        VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
-        void onInitNJointController() override;
-        void onDisconnectNJointController() override;
-        void controllerRun();
-
-    private:
-        struct DebugBufferData
-        {
-            StringFloatDictionary latestTargetVelocities;
-            StringFloatDictionary dmpTargets;
-            StringFloatDictionary realTCP;
-
-            double mpcFactor;
-            double error;
-            double phaseStop;
-            double posError;
-            double oriError;
-            double deltaT;
-            double canVal0;
-        };
-
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-        struct NJointCCDMPControllerSensorData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-        };
-        TripleBuffer<NJointCCDMPControllerSensorData> controllerSensorData;
-
-        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
-        std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
-        std::vector<ControlTarget1DoFActuatorVelocity*> targets;
-
-        // velocity ik controller parameters
-        std::vector<pidController> torquePIDs;
-        CartesianVelocityControllerPtr tcpController;
-        std::string nodeSetName;
-
-        // dmp parameters
-        std::vector<DMP::UMITSMPPtr > dmpPtrList;
-        std::vector<double> canVals;
-        std::vector<double> timeDurations;
-        std::vector<std::string > dmpTypes;
-        std::vector<double> amplitudes;
-
-        std::vector<DMP::Vec<DMP::DMPState > > currentStates;
-        std::vector<DMP::DVec > targetSubStates;
-
-        bool finished;
-        double tau;
-        ViaPointsSet viaPoints;
-        bool isDisturbance;
-
-
-        // phaseStop parameters
-        double phaseL;
-        double phaseK;
-        double phaseDist0;
-        double phaseDist1;
-        double phaseKpPos;
-        double phaseKpOri;
-
-        double posToOriRatio;
-
-        Eigen::VectorXf targetVels;
-
-        std::vector<int> learnedDMP;
-
-
-
-
-
-        NJointCCDMPControllerConfigPtr cfg;
-        VirtualRobot::RobotNodePtr tcp;
-        Eigen::Vector3f tcpPosition;
-        Eigen::Quaterniond tcpOrientation;
-
-        Eigen::Matrix4f oldPose;
-        VirtualRobot::DifferentialIKPtr ik;
-
-        DMP::DVec targetState;
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointCCDMPController>::pointer_type controllerTask;
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
deleted file mode 100644
index e5f7df8ef3a01bc95d68f4e59f8fe39b043a176a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-#include "NJointJSDMPController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-#include <boost/archive/text_oarchive.hpp>
-#include <boost/archive/text_iarchive.hpp>
-
-
-namespace armarx
-{
-
-    NJointControllerRegistration<NJointJSDMPController> registrationControllerNJointJSDMPController("NJointJSDMPController");
-
-    std::string NJointJSDMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointJSDMPController";
-    }
-
-    NJointJSDMPController::NJointJSDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "creating joint space dmp controller ... ";
-        useSynchronizedRtRobot();
-
-        cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointJointSpaceDMPControllerConfigPtr";
-
-        for (std::string jointName : cfg->jointNames)
-        {
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
-            positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
-            velocitySensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorVelocity>()));
-        }
-        if (cfg->jointNames.size() == 0)
-        {
-            ARMARX_ERROR << "cfg->jointNames.size() == 0";
-        }
-        ARMARX_INFO << "start creating dmpPtr ... " << " baseMode: " << cfg->baseMode;
-
-        dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, 100, cfg->baseMode, 1));
-        timeDuration = cfg->timeDuration;
-        phaseL = cfg->phaseL;
-        phaseK = cfg->phaseK;
-        phaseDist0 = cfg->phaseDist0;
-        phaseDist1 = cfg->phaseDist1;
-        phaseKp = cfg->phaseKp;
-        dimNames = cfg->jointNames;
-        ARMARX_INFO << "created dmpPtr ... ";
-
-        targetVels.resize(cfg->jointNames.size());
-        NJointJSDMPControllerControlData initData;
-        initData.targetJointVels.resize(cfg->jointNames.size());
-        for (size_t i = 0; i < cfg->jointNames.size(); ++i)
-        {
-            initData.targetJointVels[i] = 0;
-            targetVels[i] = 0;
-        }
-
-        reinitTripleBuffer(initData);
-
-
-        NJointJSDMPControllerSensorData initSensorData;
-        initSensorData.currentTime = 0;
-        initSensorData.deltaT = 0;
-        initSensorData.currentState.resize(cfg->jointNames.size());
-        controllerSensorData.reinitAllBuffers(initSensorData);
-
-        deltaT = 0;
-
-        qpos.resize(dimNames.size());
-        qvel.resize(dimNames.size());
-    }
-
-    void NJointJSDMPController::controllerRun()
-    {
-        if (!started || finished)
-        {
-            for (size_t i = 0; i < dimNames.size(); ++i)
-            {
-                targetVels[i] = 0;
-            }
-        }
-        else
-        {
-            currentState = controllerSensorData.getUpToDateReadBuffer().currentState;
-            double deltaT = controllerSensorData.getUpToDateReadBuffer().deltaT;
-
-            if (canVal > 1e-8)
-            {
-                double phaseStop = 0;
-                double mpcFactor = 1;
-
-                std::vector<double> currentPosition;
-                double error = 0;
-                currentPosition.resize(dimNames.size());
-
-                for (size_t i = 0; i < currentState.size(); i++)
-                {
-                    DMP::DMPState currentPos = currentState[i];
-                    currentPosition[i] = currentPos.pos;
-                    error += pow(currentPos.pos - targetState[i], 2);
-                }
-
-                if (cfg->isPhaseStop)
-                {
-                    double phaseDist;
-
-                    if (isDisturbance)
-                    {
-                        phaseDist = phaseDist1;
-                    }
-                    else
-                    {
-                        phaseDist = phaseDist0;
-                    }
-
-                    error = sqrt(error);
-                    phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
-                    mpcFactor = 1 - (phaseStop / phaseL);
-
-                    if (mpcFactor < 0.1)
-                    {
-                        isDisturbance = true;
-                    }
-
-                    if (mpcFactor > 0.9)
-                    {
-                        isDisturbance = false;
-                    }
-                }
-
-                canVal -= tau * deltaT * 1 / (1 + phaseStop);
-                double dmpDeltaT = deltaT / timeDuration;
-
-                currentDMPState = dmpPtr->calculateDirectlyVelocity(currentDMPState, canVal / timeDuration, dmpDeltaT, targetState);
-
-                for (size_t i = 0; i < currentDMPState.size(); ++i)
-                {
-                    double vel0 = tau * currentDMPState[i].vel / timeDuration;
-                    double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
-                    //                    double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
-                    double vel = vel1 + vel0;
-                    targetVels[i] = vel;
-                    debugOutputData.getWriteBuffer().latestTargetVelocities[dimNames[i]] = (float)vel;
-                    debugOutputData.getWriteBuffer().latestTargets[dimNames[i]] = (float)currentDMPState[i].pos;
-
-                }
-
-                debugOutputData.getWriteBuffer().currentCanVal = canVal;
-                debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
-                debugOutputData.commitWrite();
-
-            }
-            else
-            {
-                finished = true;
-                for (size_t i = 0; i < dimNames.size(); ++i)
-                {
-                    targetVels[i] = 0;
-                }
-            }
-        }
-
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().targetJointVels = targetVels;
-        writeControlStruct();
-    }
-
-    void NJointJSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        for (size_t i = 0; i < dimNames.size(); i++)
-        {
-            const auto& jointName = dimNames.at(i);
-            DMP::DMPState currentPos;
-            currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
-            currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
-            qpos[i] = currentPos.pos;
-            qvel[i] = currentPos.vel;
-            controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
-        }
-        controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
-        controllerSensorData.getWriteBuffer().currentTime += timeSinceLastIteration.toSecondsDouble();
-        controllerSensorData.commitWrite();
-
-
-        rt2UserData.getWriteBuffer().qpos = qpos;
-        rt2UserData.getWriteBuffer().qvel = qvel;
-        rt2UserData.commitWrite();
-
-        Eigen::VectorXf targetJointVels = rtGetControlStruct().targetJointVels;
-        //        ARMARX_INFO << targetJointVels;
-
-        for (size_t i = 0; i < dimNames.size(); ++i)
-        {
-
-            if (fabs(targetJointVels[i]) > cfg->maxJointVel)
-            {
-                targets[dimNames[i]]->velocity = targetJointVels[i] < 0 ? -cfg->maxJointVel : cfg->maxJointVel;
-            }
-            else
-            {
-                targets[dimNames[i]]->velocity = targetJointVels[i];
-            }
-
-        }
-
-
-    }
-
-    void NJointJSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
-        DMP::DVec ratios;
-        for (size_t i = 0; i < fileNames.size(); ++i)
-        {
-            DMP::SampledTrajectoryV2 traj;
-            traj.readFromCSVFile(fileNames.at(i));
-            traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
-            trajs.push_back(traj);
-
-            if (i == 0)
-            {
-                ratios.push_back(1.0);
-            }
-            else
-            {
-                ratios.push_back(0.0);
-            }
-        }
-        dmpPtr->learnFromTrajectories(trajs);
-        dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
-
-        ARMARX_INFO << "Learned DMP ... ";
-    }
-
-    void NJointJSDMPController::runDMP(const Ice::DoubleSeq& goals, double times, const Ice::Current&)
-    {
-        while (!rt2UserData.updateReadBuffer())
-        {
-            usleep(100);
-        }
-
-        targetState.clear();
-        targetState.resize(dimNames.size());
-        currentState.clear();
-        currentState.resize(dimNames.size());
-        currentDMPState.clear();
-        currentDMPState.resize(dimNames.size());
-
-        std::vector<double> goalVec = goals;
-        for (size_t i = 0; i < dimNames.size(); i++)
-        {
-            DMP::DMPState currentPos;
-            currentPos.pos =  rt2UserData.getReadBuffer().qpos[i];
-            currentPos.vel =  rt2UserData.getReadBuffer().qvel[i];
-
-            currentState[i] = currentPos;
-            currentDMPState[i] = currentPos;
-
-            targetState.push_back(currentPos.pos);
-
-            if (rtGetRobot()->getRobotNode(dimNames[i])->isLimitless())
-            {
-                double tjv = goalVec[i];
-                double cjv = currentPos.pos;
-                double diff = std::fmod(tjv - cjv, 2 * M_PI);
-                if (fabs(diff) > M_PI)
-                {
-                    if (signbit(diff))
-                    {
-                        diff =  - 2 * M_PI - diff;
-                    }
-                    else
-                    {
-                        diff = 2 * M_PI - diff;
-                    }
-                    tjv = cjv - diff;
-                }
-                else
-                {
-                    tjv = cjv + diff;
-                }
-
-                goalVec[i] = tjv;
-                ARMARX_INFO << "dim name: " << dimNames[i] <<  " current state: qpos: " << currentPos.pos << " orig target: " << goals[i] << " current goal: " << tjv;
-            }
-
-
-        }
-
-        dmpPtr->prepareExecution(goalVec, currentDMPState, 1,  1);
-        canVal = timeDuration;
-        finished = false;
-        isDisturbance = false;
-
-        tau = times;
-        ARMARX_INFO << "run DMP";
-        started = true;
-
-    }
-
-    void NJointJSDMPController::showMessages(const Ice::Current&)
-    {
-    }
-
-    std::string NJointJSDMPController::getDMPAsString(const Ice::Current&)
-    {
-        std::stringstream ss;
-        boost::archive::text_oarchive oa{ss};
-        oa << dmpPtr.get();
-        return ss.str();
-    }
-
-    std::vector<double> NJointJSDMPController::createDMPFromString(const std::string& dmpString, const Ice::Current&)
-    {
-        std::stringstream ss;
-        ss.str(dmpString);
-        boost::archive::text_iarchive ia{ss};
-        DMP::UMIDMP* newDmpPtr;
-        ia >> newDmpPtr;
-        dmpPtr.reset(newDmpPtr);
-        return dmpPtr->defaultGoal;
-    }
-
-    void NJointJSDMPController::setViaPoints(Ice::Double u, double viapoint, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpPtr->setViaPoint(u, viapoint);
-    }
-
-    void NJointJSDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
-    {
-        dmpPtr->setWeights(weights);
-    }
-
-    DoubleSeqSeq NJointJSDMPController::getMPWeights(const Ice::Current&)
-    {
-        DMP::DVec2d res = dmpPtr->getWeights();
-        DoubleSeqSeq resvec;
-        for (size_t i = 0; i < res.size(); ++i)
-        {
-            std::vector<double> cvec;
-            for (size_t j = 0; j < res[i].size(); ++j)
-            {
-                cvec.push_back(res[i][j]);
-            }
-            resvec.push_back(cvec);
-        }
-
-        return resvec;
-    }
-
-    void NJointJSDMPController::setSpeed(double times, const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-        tau = times;
-    }
-
-    void NJointJSDMPController::rtPreActivateController()
-    {
-    }
-
-    void NJointJSDMPController::rtPostDeactivateController()
-    {
-
-    }
-
-    void NJointJSDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        values = debugOutputData.getUpToDateReadBuffer().latestTargets;
-        for (auto& pair : values)
-        {
-            datafields[pair.first + "_pos"] = new Variant(pair.second);
-        }
-
-        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
-        debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
-    }
-
-
-    void NJointJSDMPController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        started = false;
-        runTask("NJointJSDMPController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-    }
-
-    void NJointJSDMPController::onDisconnectNJointController()
-    {
-
-    }
-
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
deleted file mode 100644
index 2dce7b5344063f02d62f571c7ee33131b37f873a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ /dev/null
@@ -1,142 +0,0 @@
-
-#pragma once
-
-#include <Eigen/Dense>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
-
-namespace armarx
-{
-
-    TYPEDEF_PTRS_HANDLE(NJointJSDMPController);
-
-    TYPEDEF_PTRS_HANDLE(NJointJSDMPControllerControlData);
-    class NJointJSDMPControllerControlData
-    {
-    public:
-        Eigen::VectorXf targetJointVels;
-    };
-
-    /**
-     * @brief The NJointJSDMPController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointJSDMPController :
-        public NJointControllerWithTripleBuffer<NJointJSDMPControllerControlData>,
-        public NJointJointSpaceDMPControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
-        NJointJSDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const override;
-
-        // NJointController interface
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        bool isFinished(const Ice::Current&) override
-        {
-            return finished;
-        }
-
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
-        void setSpeed(double times, const Ice::Current&) override;
-
-        void runDMP(const Ice::DoubleSeq& goals, double times, const Ice::Current&) override;
-
-        void showMessages(const Ice::Current&) override;
-        //        std::string getDMPAsString(const Ice::Current&) override;
-        std::string getDMPAsString(const Ice::Current&) override;
-        std::vector<double> createDMPFromString(const std::string& dmpString, const Ice::Current&) override;
-        void setViaPoints(Ice::Double u, double viapoint, const Ice::Current&) override;
-
-        void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
-        DoubleSeqSeq getMPWeights(const Ice::Current&) override;
-
-
-    protected:
-        void rtPreActivateController() override;
-        void rtPostDeactivateController() override;
-
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-    private:
-        NJointJointSpaceDMPControllerConfigPtr cfg;
-        struct DebugBufferData
-        {
-            StringFloatDictionary latestTargetVelocities;
-            StringFloatDictionary latestTargets;
-
-            double currentCanVal;
-            double mpcFactor;
-        };
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-
-        struct NJointJSDMPControllerSensorData
-        {
-            double currentTime;
-            double deltaT;
-            DMP::Vec<DMP::DMPState> currentState;
-        };
-        TripleBuffer<NJointJSDMPControllerSensorData> controllerSensorData;
-
-        struct RTToUserData
-        {
-            Eigen::VectorXf qpos;
-            Eigen::VectorXf qvel;
-        };
-        TripleBuffer<RTToUserData> rt2UserData;
-
-
-        std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
-        std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
-
-        IceUtil::Time last;
-
-        DMP::UMIDMPPtr dmpPtr;
-        double timeDuration;
-        DMP::Vec<DMP::DMPState> currentState;
-        DMP::Vec<DMP::DMPState> currentDMPState;
-
-        double canVal;
-        double deltaT;
-        double tau;
-
-        double finished;
-
-        // phaseStop parameters
-        double phaseL;
-        double phaseK;
-        double phaseDist0;
-        double phaseDist1;
-        double phaseKp;
-
-        bool isDisturbance;
-        bool started;
-        std::vector<std::string> dimNames;
-        DMP::DVec targetState;
-        Eigen::VectorXf targetVels;
-
-        mutable MutexType controllerMutex;
-
-        Eigen::VectorXf qpos;
-        Eigen::VectorXf qvel;
-        // ManagedIceObject interface
-    protected:
-        void controllerRun();
-        void onInitNJointController() override;
-        void onDisconnectNJointController() override;
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
deleted file mode 100644
index ac8005eba8c70cdfce5aa45d80bd189ef06f15c5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
+++ /dev/null
@@ -1,247 +0,0 @@
-#include "NJointJointSpaceDMPController.h"
-
-
-
-namespace armarx
-{
-
-    NJointControllerRegistration<NJointJointSpaceDMPController> registrationControllerNJointJointSpaceDMPController("NJointJointSpaceDMPController");
-
-    std::string NJointJointSpaceDMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointJointSpaceDMPController";
-    }
-
-    NJointJointSpaceDMPController::NJointJointSpaceDMPController(armarx::RobotUnitPtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        NJointJointSpaceDMPControllerConfigPtr cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr");
-
-        for (std::string jointName : cfg->jointNames)
-        {
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
-            positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
-            torqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorTorque>()));
-            gravityTorqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFGravityTorque>()));
-            velocitySensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorVelocity>()));
-        }
-        if (cfg->jointNames.size() == 0)
-        {
-            ARMARX_ERROR << "cfg->jointNames.size() == 0";
-        }
-
-        dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
-        timeDuration = cfg->timeDuration;
-        canVal = timeDuration;
-        finished = false;
-        phaseL = cfg->phaseL;
-        phaseK = cfg->phaseK;
-        phaseDist0 = cfg->phaseDist0;
-        phaseDist1 = cfg->phaseDist1;
-        phaseKp = cfg->phaseKp;
-
-        isDisturbance = false;
-
-        NJointJointSpaceDMPControllerControlData initData;
-        initData.tau = 1.0;
-        initData.isStart = false;
-        reinitTripleBuffer(initData);
-    }
-
-    void NJointJointSpaceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        if (rtGetControlStruct().isStart && !finished)
-        {
-            currentState.clear();
-            double phaseStop = 0;
-            double error = 0;
-            std::vector<double> currentPosition;
-            std::vector<double> currentVelocity;
-            for (size_t i = 0; i < dimNames.size(); i++)
-            {
-                const auto& jointName = dimNames.at(i);
-                DMP::DMPState currentPos;
-                currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
-                currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
-                currentPos.vel *= timeDuration;
-                currentState.push_back(currentPos);
-                currentPosition.push_back(currentPos.pos);
-                currentVelocity.push_back(currentPos.vel);
-
-                error += pow(currentPos.pos - targetState[i], 2);
-            }
-
-            double phaseDist;
-
-            if (isDisturbance)
-            {
-                phaseDist = phaseDist1;
-            }
-            else
-            {
-                phaseDist = phaseDist0;
-            }
-
-            error = sqrt(error);
-            phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
-            mpcFactor = 1 - (phaseStop / phaseL);
-
-            if (mpcFactor < 0.1)
-            {
-                isDisturbance = true;
-            }
-
-            if (mpcFactor > 0.9)
-            {
-                isDisturbance = false;
-            }
-
-            double tau = rtGetControlStruct().tau;
-            double deltaT = timeSinceLastIteration.toSecondsDouble();
-            canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop);
-            double dmpDeltaT = deltaT / timeDuration;
-            dmpPtr->setTemporalFactor(tau);
-
-            currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
-
-            if (canVal < 1e-8)
-            {
-                finished = true;
-            }
-
-            for (size_t i = 0; i < dimNames.size(); ++i)
-            {
-                const auto& jointName = dimNames.at(i);
-                if (targets.count(jointName) == 1)
-                {
-                    double vel0 = currentState[i].vel / timeDuration;
-                    double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
-                    double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
-                    targets[jointName]->velocity = finished ? 0.0f : vel;
-
-                    std::string targetVelstr = jointName + "_targetvel";
-                    std::string targetStatestr = jointName + "_dmpTarget";
-                    debugOutputData.getWriteBuffer().latestTargetVelocities[jointName] = vel;
-                    debugOutputData.getWriteBuffer().dmpTargetState[jointName] = targetState[i];
-
-                }
-            }
-
-            debugOutputData.getWriteBuffer().currentCanVal = canVal;
-            debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
-            debugOutputData.commitWrite();
-        }
-        else
-        {
-            for (size_t i = 0; i < dimNames.size(); ++i)
-            {
-                const auto& jointName = dimNames.at(i);
-                if (targets.count(jointName) == 1)
-                {
-                    targets[jointName]->velocity =  0.0f;
-                }
-            }
-        }
-    }
-
-    void NJointJointSpaceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
-        DMP::DVec ratios;
-        for (size_t i = 0; i < fileNames.size(); ++i)
-        {
-            DMP::SampledTrajectoryV2 traj;
-            traj.readFromCSVFile(fileNames.at(i));
-            dimNames = traj.getDimensionNames();
-            traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
-            trajs.push_back(traj);
-
-            if (i == 0)
-            {
-                ratios.push_back(1.0);
-            }
-            else
-            {
-                ratios.push_back(0.0);
-            }
-        }
-        dmpPtr->learnFromTrajectories(trajs);
-        dmpPtr->setOneStepMPC(true);
-        dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
-
-        ARMARX_INFO << "Learned DMP ... ";
-    }
-
-    void NJointJointSpaceDMPController::runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&)
-    {
-        currentState.clear();
-        targetState.clear();
-        for (size_t i = 0; i < dimNames.size(); i++)
-        {
-            const auto& jointName = dimNames.at(i);
-            DMP::DMPState currentPos;
-            currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
-            currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
-            currentState.push_back(currentPos);
-            targetState.push_back(currentPos.pos);
-        }
-        dmpPtr->prepareExecution(goals, currentState, 1,  tau);
-        finished = false;
-
-        this->goals = goals;
-        this->tau = tau;
-
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().tau = tau;
-        getWriterControlStruct().isStart = true;
-        writeControlStruct();
-
-    }
-
-    void NJointJointSpaceDMPController::showMessages(const Ice::Current&)
-    {
-    }
-
-    void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
-    {
-        this->tau = tau;
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().tau = tau;
-        getWriterControlStruct().isStart = true;
-        writeControlStruct();
-    }
-
-    void NJointJointSpaceDMPController::rtPreActivateController()
-    {
-    }
-
-    void NJointJointSpaceDMPController::rtPostDeactivateController()
-    {
-
-    }
-
-    void NJointJointSpaceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        for (auto& pair : values)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        auto valuesst = debugOutputData.getUpToDateReadBuffer().dmpTargetState;
-        for (auto& pair : valuesst)
-        {
-            datafields[pair.first] = new Variant(pair.second);
-        }
-
-        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
-        debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
-    }
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
deleted file mode 100644
index 09eb7f2af51d889dc5715fd8b14e98332c67a0ad..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
+++ /dev/null
@@ -1,118 +0,0 @@
-
-#pragma once
-
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
-
-namespace armarx
-{
-
-    TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPController);
-
-    TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPControllerControlData);
-    class NJointJointSpaceDMPControllerControlData
-    {
-    public:
-        double tau;
-        bool isStart;
-    };
-
-
-    //    class SimplePID
-    //    {
-    //    public:
-    //        float Kp = 0, Kd = 0;
-    //        float lastError = 0;
-    //        float update(float dt, float error);
-    //    };
-
-    /**
-     * @brief The NJointJointSpaceDMPController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointJointSpaceDMPController :
-        public NJointControllerWithTripleBuffer<NJointJointSpaceDMPControllerControlData>,
-        public NJointJointSpaceDMPControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
-        NJointJointSpaceDMPController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const override;
-
-        // NJointController interface
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        //
-        bool isFinished(const Ice::Current&) override
-        {
-            return finished;
-        }
-
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
-        void setTemporalFactor(double tau, const Ice::Current&) override;
-
-        void runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&) override;
-
-        void showMessages(const Ice::Current&) override;
-
-    protected:
-        void rtPreActivateController() override;
-        void rtPostDeactivateController() override;
-
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-    private:
-
-        struct DebugBufferData
-        {
-            StringFloatDictionary latestTargetVelocities;
-            StringFloatDictionary dmpTargetState;
-            double currentCanVal;
-            double mpcFactor;
-        };
-
-        std::map<std::string, const SensorValue1DoFActuatorTorque*> torqueSensors;
-        std::map<std::string, const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
-        std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
-        std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
-
-
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-
-        std::vector<double> goals;
-        DMP::UMIDMPPtr dmpPtr;
-        bool DMPAsForwardControl;
-        double timeDuration;
-
-        double canVal;
-
-        double tau;
-        double finished;
-
-        // phaseStop parameters
-        double phaseL;
-        double phaseK;
-        double phaseDist0;
-        double phaseDist1;
-        double phaseKp;
-
-        double mpcFactor;
-
-        bool isDisturbance;
-        std::vector<std::string> dimNames;
-        DMP::Vec<DMP::DMPState> currentState;
-        DMP::DVec targetState;
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
deleted file mode 100644
index e8a401f485b38a2c85f4a9a718f94d8a1d0a905a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ /dev/null
@@ -1,737 +0,0 @@
-#include "NJointPeriodicTSDMPCompliantController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointPeriodicTSDMPCompliantController> registrationControllerNJointPeriodicTSDMPCompliantController("NJointPeriodicTSDMPCompliantController");
-
-    NJointPeriodicTSDMPCompliantController::NJointPeriodicTSDMPCompliantController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        useSynchronizedRtRobot();
-        cfg =  NJointPeriodicTSDMPCompliantControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
-        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
-        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-        for (size_t i = 0; i < rns->getSize(); ++i)
-        {
-            std::string jointName = rns->getNode(i)->getName();
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            velocitySensors.push_back(velocitySensor);
-            positionSensors.push_back(positionSensor);
-        };
-
-        tcp = rns->getTCP();
-        // set tcp controller
-        nodeSetName = cfg->nodeSetName;
-        ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
-        taskSpaceDMPConfig.DMPMode = "Linear";
-        taskSpaceDMPConfig.DMPStyle = "Periodic";
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-
-
-
-        dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
-
-        NJointPeriodicTSDMPCompliantControllerControlData initData;
-        initData.targetTSVel.resize(6);
-        for (size_t i = 0; i < 6; ++i)
-        {
-            initData.targetTSVel(i) = 0;
-        }
-        reinitTripleBuffer(initData);
-
-        firstRun = true;
-        dmpRunning = false;
-
-
-        ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
-        ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
-        ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
-        ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
-
-        kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
-        dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
-        kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
-        dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
-        kpf = cfg->Kpf;
-        knull = cfg->Knull;
-        dnull = cfg->Dnull;
-
-
-
-        nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
-        for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
-        {
-            nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
-        }
-
-
-        const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
-        forceSensor = svlf->asA<SensorValueForceTorque>();
-
-        forceOffset.setZero();
-        filteredForce.setZero();
-        filteredForceInRoot.setZero();
-
-
-        UserToRTData initUserData;
-        initUserData.targetForce = 0;
-        user2rtData.reinitAllBuffers(initUserData);
-
-        oriToolDir << 0, 0, 1;
-
-        qvel_filtered.setZero(targets.size());
-
-        ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
-        ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
-        ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
-
-        // only for ARMAR-6 (safe-guard)
-        if (!cfg->ignoreWSLimitChecks)
-        {
-            ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
-            ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
-            ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
-
-            ARMARX_CHECK_LESS(cfg->ws_y[0],  cfg->ws_y[1]);
-            ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
-            ARMARX_CHECK_LESS(0,  cfg->ws_y[1]);
-
-            ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
-            ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
-            ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
-        }
-
-        adaptK = kpos;
-        lastDiff = 0;
-        changeTimer = 0;
-
-
-        isManipulability = cfg->isManipulability;
-
-
-        ARMARX_CHECK_EQUAL(cfg->maniWeight.size(), rns->getNodeNames().size());
-        Eigen::VectorXd maniWeightVec;
-        maniWeightVec.setOnes(rns->getNodeNames().size());
-        for (size_t i = 0; i < cfg->maniWeight.size(); ++i)
-        {
-            maniWeightVec(i) = cfg->maniWeight[i];
-        }
-
-        Eigen::MatrixXd maniWeightMat = maniWeightVec.asDiagonal();
-        VirtualRobot::SingleRobotNodeSetManipulabilityPtr manipulability(
-            new VirtualRobot::SingleRobotNodeSetManipulability(rns, rns->getTCP(),
-                    VirtualRobot::AbstractManipulability::Mode::Position,
-                    VirtualRobot::AbstractManipulability::Type::Velocity,
-                    rns->getRobot()->getRootNode(), maniWeightMat));
-        manipulabilityTracker.reset(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
-
-        ARMARX_CHECK_EQUAL(cfg->positionManipulability.size(), 9);
-        targetManipulability.setZero(3, 3);
-        targetManipulability << cfg->positionManipulability[0],  cfg->positionManipulability[1], cfg->positionManipulability[2],
-                             cfg->positionManipulability[3],  cfg->positionManipulability[4], cfg->positionManipulability[5],
-                             cfg->positionManipulability[6],  cfg->positionManipulability[7], cfg->positionManipulability[8];
-
-
-        Eigen::VectorXd kmaniVec;
-        kmaniVec.setZero(cfg->kmani.size());
-        for (size_t i = 0; i < cfg->kmani.size(); ++i)
-        {
-            kmaniVec[i] = cfg->kmani[i];
-        }
-
-        kmani = kmaniVec.asDiagonal();
-
-    }
-
-    void NJointPeriodicTSDMPCompliantController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-
-
-        RTToControllerData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentPose = tcp->getPoseInRootFrame();
-        initSensorData.currentTwist.setZero();
-        initSensorData.isPhaseStop = false;
-        rt2CtrlData.reinitAllBuffers(initSensorData);
-
-        RTToUserData initInterfaceData;
-        initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
-        initInterfaceData.waitTimeForCalibration = 0;
-        rt2UserData.reinitAllBuffers(initInterfaceData);
-
-
-        ARMARX_IMPORTANT << "read force sensor ...";
-
-        forceOffset = forceSensor->force;
-
-        ARMARX_IMPORTANT << "force offset: " << forceOffset;
-
-        started = false;
-
-        runTask("NJointPeriodicTSDMPCompliantController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-
-        ARMARX_IMPORTANT << "started controller ";
-
-    }
-
-    std::string NJointPeriodicTSDMPCompliantController::getClassName(const Ice::Current&) const
-    {
-        return "NJointPeriodicTSDMPCompliantController";
-    }
-
-    void NJointPeriodicTSDMPCompliantController::controllerRun()
-    {
-        if (!started)
-        {
-            return;
-        }
-
-        if (!dmpCtrl)
-        {
-            return;
-        }
-
-        Eigen::VectorXf targetVels(6);
-        bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
-        if (isPhaseStop)
-        {
-            targetVels.setZero();
-        }
-        else
-        {
-
-            double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
-            Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
-            Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
-
-            LockGuardType guard {controllerMutex};
-            dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
-            targetVels = dmpCtrl->getTargetVelocity();
-
-            debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
-            debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
-            VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
-            debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
-            debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
-            debugOutputData.getWriteBuffer().mpcFactor =  dmpCtrl->debugData.mpcFactor;
-            debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
-            debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
-            debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
-            debugOutputData.getWriteBuffer().deltaT = deltaT;
-            debugOutputData.commitWrite();
-        }
-
-        getWriterControlStruct().targetTSVel = targetVels;
-        writeControlStruct();
-
-        dmpRunning = true;
-    }
-
-
-    void NJointPeriodicTSDMPCompliantController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
-        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
-        rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
-        rt2UserData.commitWrite();
-
-
-        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf qpos(positionSensors.size());
-        Eigen::VectorXf qvel(velocitySensors.size());
-        for (size_t i = 0; i < positionSensors.size(); ++i)
-        {
-            qpos(i) = positionSensors[i]->position;
-            qvel(i) = velocitySensors[i]->velocity;
-        }
-
-        qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
-        Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
-
-        Eigen::VectorXf targetVel(6);
-        targetVel.setZero();
-        if (firstRun || !dmpRunning)
-        {
-            lastPosition = currentPose.block<2, 1>(0, 3);
-            targetPose = currentPose;
-            firstRun = false;
-            filteredForce.setZero();
-
-            origHandOri = currentPose.block<3, 3>(0, 0);
-            toolTransform = origHandOri.transpose();
-            // force calibration
-            if (!dmpRunning)
-            {
-                forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
-            }
-
-            targetVel.setZero();
-        }
-        else
-        {
-            // communicate with DMP controller
-            rtUpdateControlStruct();
-            targetVel = rtGetControlStruct().targetTSVel;
-            targetVel(2) = 0;
-
-            // force detection
-            filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
-
-            for (size_t i = 0; i < 3; ++i)
-            {
-                if (fabs(filteredForce(i)) > cfg->forceDeadZone)
-                {
-                    filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
-                }
-                else
-                {
-                    filteredForce(i) = 0;
-                }
-            }
-            Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
-            filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
-            float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
-
-            //            Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
-            //            Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri;
-            float desiredZVel = kpf * (targetForce - filteredForceInRoot(2));
-            targetVel(2) -= desiredZVel;
-            //            targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
-
-
-            //            Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir;
-
-            for (int i = 3; i < 6; ++i)
-            {
-                targetVel(i) = 0;
-            }
-
-            // rotation changes
-
-            //            if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
-            //            {
-            //                Eigen::Vector3f desiredToolDir = filteredForceInRoot / filteredForceInRoot.norm();
-            //                currentToolDir = currentToolDir / currentToolDir.norm();
-            //                Eigen::Vector3f axis = currentToolDir.cross(desiredToolDir);
-            //                float angle = acosf(currentToolDir.dot(desiredToolDir));
-
-            //                if (fabs(angle) < M_PI / 2)
-            //                {
-            //                    Eigen::AngleAxisf desiredToolRot(angle, axis);
-            //                    Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
-
-            //                    targetPose.block<3, 3>(0, 0) = desiredRotMat * currentHandOri;
-
-            //                    Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
-            //                    Eigen::Vector3f checkedToolDir =  desiredRotMat * currentToolDir;
-            //                    ARMARX_IMPORTANT << "axis: " << axis;
-            //                    ARMARX_IMPORTANT << "angle: " << angle * 180 / 3.1415;
-            //                    ARMARX_IMPORTANT << "desiredRotMat: " << desiredRotMat;
-
-            //                    ARMARX_IMPORTANT << "desiredToolDir: " << desiredToolDir;
-            //                    ARMARX_IMPORTANT << "currentToolDir: " << currentToolDir;
-            //                    ARMARX_IMPORTANT << "checkedToolDir: " << checkedToolDir;
-            //                }
-
-            //            }
-
-
-            // integrate for targetPose
-
-
-
-
-
-        }
-
-        bool isPhaseStop = false;
-
-        float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
-        if (diff > cfg->phaseDist0)
-        {
-            isPhaseStop = true;
-        }
-
-        float dTf = (float)deltaT;
-
-
-        if (filteredForceInRoot.block<2, 1>(0, 0).norm() > cfg->dragForceDeadZone)
-        {
-            Eigen::Vector2f dragForce = filteredForceInRoot.block<2, 1>(0, 0) - cfg->dragForceDeadZone * filteredForceInRoot.block<2, 1>(0, 0) / filteredForceInRoot.block<2, 1>(0, 0).norm();
-            adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
-            adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
-            lastDiff = diff;
-        }
-        else
-        {
-            adaptK(0) = fmin(adaptK(0) + fabs(dTf * cfg->adaptCoeff), kpos(0));
-            adaptK(1) = fmin(adaptK(1) + fabs(dTf * cfg->adaptCoeff), kpos(1));
-        }
-        adaptK(2) = kpos(2);
-
-        // adaptive control with distance
-
-
-
-
-        targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
-
-        if (isPhaseStop)
-        {
-            Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
-            if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
-            {
-                changeTimer += deltaT;
-            }
-            else
-            {
-                lastPosition = currentPose.block<2, 1>(0, 3);
-                changeTimer = 0;
-            }
-
-            if (changeTimer > cfg->changeTimerThreshold)
-            {
-                targetPose(0, 3) = currentPose(0, 3);
-                targetPose(1, 3) = currentPose(1, 3);
-                isPhaseStop = false;
-                changeTimer = 0;
-            }
-        }
-        else
-        {
-            changeTimer = 0;
-        }
-
-
-        targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
-        targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
-
-        targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
-        targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
-
-        targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
-        targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
-
-
-
-
-        debugRT.getWriteBuffer().targetPose = targetPose;
-        debugRT.getWriteBuffer().currentPose = currentPose;
-        debugRT.getWriteBuffer().filteredForce = filteredForceInRoot;
-        debugRT.getWriteBuffer().targetVel = targetVel;
-        debugRT.getWriteBuffer().adaptK = adaptK;
-        debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
-        debugRT.getWriteBuffer().currentTwist = currentTwist;
-
-        rt2CtrlData.getWriteBuffer().currentPose = currentPose;
-        rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
-        rt2CtrlData.getWriteBuffer().deltaT = deltaT;
-        rt2CtrlData.getWriteBuffer().currentTime += deltaT;
-        rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
-        rt2CtrlData.commitWrite();
-
-        //            Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
-        //            targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
-
-        // inverse dynamic controller
-
-        for (size_t ki = 0; ki < 3; ++ki)
-        {
-            jacobi.row(ki) = 0.001 * jacobi.row(ki); // convert mm to m
-
-        }
-
-
-
-
-        Eigen::Vector6f jointControlWrench;
-        {
-            Eigen::Vector3f targetTCPLinearVelocity;
-            targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
-            Eigen::Vector3f currentTCPLinearVelocity;
-            currentTCPLinearVelocity <<  0.001 * currentTwist(0),  0.001 * currentTwist(1),   0.001 * currentTwist(2);
-            Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
-            Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
-
-            Eigen::Vector3f linearVel = adaptK.cwiseProduct(desiredPosition - currentTCPPosition);
-
-            //            if (isPhaseStop)
-            //            {
-            //                linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
-            //                for (size_t i = 0; i < 3; ++i)
-            //                {
-            //                    linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
-            //                }
-            //            }
-            //            else
-            //            {
-            //                linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
-            //            }
-            Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
-
-            Eigen::Vector3f currentTCPAngularVelocity;
-            currentTCPAngularVelocity << currentTwist(3),   currentTwist(4),  currentTwist(5);
-            Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
-            Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-            Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) + dori.cwiseProduct(- currentTCPAngularVelocity);
-            jointControlWrench <<  tcpDesiredForce, tcpDesiredTorque;
-        }
-
-
-
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
-        Eigen::VectorXf nullspaceTorque;
-
-        float manidist = 0;
-        if (isManipulability)
-        {
-            nullspaceTorque = manipulabilityTracker->calculateVelocity(targetManipulability, kmani, true);
-            manidist = manipulabilityTracker->computeDistance(targetManipulability);
-        }
-        else
-        {
-            nullspaceTorque = knull * (nullSpaceJointsVec - qpos) - dnull * qvel;
-        }
-
-        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
-        Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
-        // torque filter (maybe)
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            targets.at(i)->torque = jointDesiredTorques(i);
-
-            if (!targets.at(i)->isValid())
-            {
-                targets.at(i)->torque = 0.0f;
-            }
-            else
-            {
-                if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
-                {
-                    targets.at(i)->torque = fabs(cfg->maxJointTorque) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
-                }
-            }
-        }
-
-        debugRT.getWriteBuffer().manidist = manidist;
-
-        debugRT.commitWrite();
-
-
-    }
-
-
-    void NJointPeriodicTSDMPCompliantController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        ARMARX_INFO << "Learning DMP ... ";
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->learnDMPFromFiles(fileNames);
-
-    }
-
-    void NJointPeriodicTSDMPCompliantController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
-    {
-        ARMARX_INFO << "Learning DMP ... ";
-        ARMARX_CHECK_EXPRESSION(trajectory);
-        TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
-        ARMARX_CHECK_EXPRESSION(dmpTraj);
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->learnDMPFromTrajectory(dmpTraj);
-
-    }
-
-    void NJointPeriodicTSDMPCompliantController::setSpeed(Ice::Double times, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setSpeed(times);
-    }
-
-
-    void NJointPeriodicTSDMPCompliantController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setGoalPoseVec(goals);
-    }
-
-    void NJointPeriodicTSDMPCompliantController::setAmplitude(Ice::Double amp, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setAmplitude(amp);
-    }
-
-    void NJointPeriodicTSDMPCompliantController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
-    {
-        LockGuardType guard {controlDataMutex};
-        user2rtData.getWriteBuffer().targetForce = targetForce;
-        user2rtData.commitWrite();
-    }
-
-    void NJointPeriodicTSDMPCompliantController::runDMP(const Ice::DoubleSeq&  goals, Ice::Double tau, const Ice::Current&)
-    {
-        firstRun = true;
-        while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
-        {
-            usleep(100);
-        }
-
-
-        Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-        dmpCtrl->setSpeed(tau);
-
-        ARMARX_IMPORTANT << "run DMP";
-        started = true;
-        dmpRunning = false;
-    }
-
-
-    void NJointPeriodicTSDMPCompliantController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-        std::string datafieldName;
-        std::string debugName = "Periodic";
-        StringVariantBaseMap datafields;
-
-        Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
-        datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
-        datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
-        datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
-
-        Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
-        datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
-        datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
-        datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
-
-        Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
-        datafields["filteredforce_x"] = new Variant(filteredForce(0));
-        datafields["filteredforce_y"] = new Variant(filteredForce(1));
-        datafields["filteredforce_z"] = new Variant(filteredForce(2));
-
-
-        Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
-        datafields["reactForce_x"] = new Variant(reactForce(0));
-        datafields["reactForce_y"] = new Variant(reactForce(1));
-        datafields["reactForce_z"] = new Variant(reactForce(2));
-
-        Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
-        datafields["targetVel_x"] = new Variant(targetVel(0));
-        datafields["targetVel_y"] = new Variant(targetVel(1));
-        datafields["targetVel_z"] = new Variant(targetVel(2));
-
-        Eigen::VectorXf currentVel = debugRT.getUpToDateReadBuffer().currentTwist;
-        datafields["currentVel_x"] = new Variant(currentVel(0));
-        datafields["currentVel_y"] = new Variant(currentVel(1));
-        datafields["currentVel_z"] = new Variant(currentVel(2));
-
-        Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
-        datafields["adaptK_x"] = new Variant(adaptK(0));
-        datafields["adaptK_y"] = new Variant(adaptK(1));
-
-        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-        datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
-        datafields["manidist"] = new Variant(debugRT.getUpToDateReadBuffer().manidist);
-
-
-        //        datafields["targetVel_rx"] = new Variant(targetVel(3));
-        //        datafields["targetVel_ry"] = new Variant(targetVel(4));
-        //        datafields["targetVel_rz"] = new Variant(targetVel(5));
-
-        //        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        //        for (auto& pair : values)
-        //        {
-        //            datafieldName = pair.first  + "_" + debugName;
-        //            datafields[datafieldName] = new Variant(pair.second);
-        //        }
-
-        //        auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
-        //        for (auto& pair : currentPose)
-        //        {
-        //            datafieldName = pair.first + "_" + debugName;
-        //            datafields[datafieldName] = new Variant(pair.second);
-        //        }
-
-        //        datafieldName = "canVal_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        //        datafieldName = "mpcFactor_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
-        //        datafieldName = "error_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
-        //        datafieldName = "posError_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
-        //        datafieldName = "oriError_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
-        //        datafieldName = "deltaT_" + debugName;
-        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-        datafieldName = "PeriodicDMP";
-        debugObs->setDebugChannel(datafieldName, datafields);
-    }
-
-
-
-    void NJointPeriodicTSDMPCompliantController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
deleted file mode 100644
index 477feb6900efcdaca0981deb92534cc5dfb0c172..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ /dev/null
@@ -1,188 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-#include <VirtualRobot/Manipulability/SingleChainManipulability.h>
-#include <VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h>
-namespace armarx
-{
-
-
-    TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPCompliantController);
-    TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPCompliantControllerControlData);
-
-    class NJointPeriodicTSDMPCompliantControllerControlData
-    {
-    public:
-        Eigen::VectorXf targetTSVel;
-    };
-
-    /**
-     * @brief The NJointPeriodicTSDMPCompliantController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointPeriodicTSDMPCompliantController :
-        public NJointControllerWithTripleBuffer<NJointPeriodicTSDMPCompliantControllerControlData>,
-        public NJointPeriodicTSDMPCompliantControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointPeriodicTSDMPCompliantControllerConfigPtr;
-        NJointPeriodicTSDMPCompliantController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointPeriodicTSDMPCompliantControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
-        void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
-        bool isFinished(const Ice::Current&)
-        {
-            return false;
-        }
-
-        void setSpeed(Ice::Double times, const Ice::Current&);
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void setAmplitude(Ice::Double amp, const Ice::Current&);
-        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
-        void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
-        double getCanVal(const Ice::Current&)
-        {
-            return dmpCtrl->canVal;
-        }
-
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitNJointController();
-        void onDisconnectNJointController();
-        void controllerRun();
-
-    private:
-        struct DebugBufferData
-        {
-            StringFloatDictionary latestTargetVelocities;
-            StringFloatDictionary currentPose;
-            double currentCanVal;
-            double mpcFactor;
-            double error;
-            double phaseStop;
-            double posError;
-            double oriError;
-            double deltaT;
-        };
-
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-
-        struct DebugRTData
-        {
-            Eigen::Matrix4f targetPose;
-            Eigen::Vector3f filteredForce;
-            Eigen::Vector3f reactForce;
-            Eigen::Vector3f adaptK;
-            Eigen::VectorXf targetVel;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-            bool isPhaseStop;
-            float manidist;
-        };
-        TripleBuffer<DebugRTData> debugRT;
-
-        struct RTToControllerData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-            bool isPhaseStop;
-        };
-        TripleBuffer<RTToControllerData> rt2CtrlData;
-
-        struct RTToUserData
-        {
-            Eigen::Matrix4f currentTcpPose;
-            float waitTimeForCalibration;
-        };
-        TripleBuffer<RTToUserData> rt2UserData;
-
-        struct UserToRTData
-        {
-            float targetForce;
-        };
-        TripleBuffer<UserToRTData> user2rtData;
-
-
-        TaskSpaceDMPControllerPtr dmpCtrl;
-
-        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-        std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
-        // velocity ik controller parameters
-        std::string nodeSetName;
-
-        bool started;
-        bool firstRun;
-        bool dmpRunning;
-
-        VirtualRobot::DifferentialIKPtr ik;
-        VirtualRobot::RobotNodePtr tcp;
-
-        NJointPeriodicTSDMPCompliantControllerConfigPtr cfg;
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointPeriodicTSDMPCompliantController>::pointer_type controllerTask;
-        Eigen::Matrix4f targetPose;
-
-        Eigen::Vector3f kpos;
-        Eigen::Vector3f dpos;
-        Eigen::Vector3f kori;
-        Eigen::Vector3f dori;
-        float knull;
-        float dnull;
-        float kpf;
-
-        Eigen::VectorXf nullSpaceJointsVec;
-        const SensorValueForceTorque* forceSensor;
-
-        Eigen::Vector3f filteredForce;
-        Eigen::Vector3f forceOffset;
-        Eigen::Vector3f filteredForceInRoot;
-
-        Eigen::Matrix3f toolTransform;
-        Eigen::Vector3f oriToolDir;
-        Eigen::Matrix3f origHandOri;
-        Eigen::VectorXf qvel_filtered;
-
-        Eigen::Vector3f adaptK;
-        float lastDiff;
-        Eigen::Vector2f lastPosition;
-        double changeTimer;
-
-
-
-        bool isManipulability = false;
-        VirtualRobot::SingleChainManipulabilityTrackingPtr manipulabilityTracker;
-        Eigen::MatrixXd targetManipulability;
-        Eigen::MatrixXd kmani;
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
deleted file mode 100644
index abbebd242dd5b6b8755eff620d06af9dcf854922..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
+++ /dev/null
@@ -1,429 +0,0 @@
-#include "NJointPeriodicTSDMPForwardVelController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointPeriodicTSDMPForwardVelController> registrationControllerNJointPeriodicTSDMPForwardVelController("NJointPeriodicTSDMPForwardVelController");
-
-    NJointPeriodicTSDMPForwardVelController::NJointPeriodicTSDMPForwardVelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        useSynchronizedRtRobot();
-        cfg = NJointPeriodicTSDMPControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
-        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
-        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-        for (size_t i = 0; i < rns->getSize(); ++i)
-        {
-            std::string jointName = rns->getNode(i)->getName();
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            velocitySensors.push_back(velocitySensor);
-            positionSensors.push_back(positionSensor);
-        };
-
-        tcp = rns->getTCP();
-        // set tcp controller
-        tcpController.reset(new CartesianVelocityController(rns, tcp));
-        nodeSetName = cfg->nodeSetName;
-        ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
-
-        taskSpaceDMPConfig.DMPMode = "Linear";
-        taskSpaceDMPConfig.DMPStyle = "Periodic";
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-
-
-        dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
-
-        NJointPeriodicTSDMPForwardVelControllerControlData initData;
-        initData.targetPose = tcp->getPoseInRootFrame();
-        initData.targetTSVel.resize(6);
-        for (size_t i = 0; i < 6; ++i)
-        {
-            initData.targetTSVel(i) = 0;
-        }
-        reinitTripleBuffer(initData);
-
-        finished = false;
-        firstRun = true;
-
-
-        const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
-        forceSensor = svlf->asA<SensorValueForceTorque>();
-
-        forceOffset.setZero();
-        filteredForce.setZero();
-
-        UserToRTData initUserData;
-        initUserData.targetForce = 0;
-        user2rtData.reinitAllBuffers(initUserData);
-
-        oriToolDir << 0, 0, 1;
-
-        kpf = cfg->Kpf;
-
-    }
-
-    std::string NJointPeriodicTSDMPForwardVelController::getClassName(const Ice::Current&) const
-    {
-        return "NJointPeriodicTSDMPForwardVelController";
-    }
-
-    void NJointPeriodicTSDMPForwardVelController::controllerRun()
-    {
-        if (!started)
-        {
-            return;
-        }
-
-        if (!rt2CtrlData.updateReadBuffer() || !dmpCtrl)
-        {
-            return;
-        }
-
-        double deltaT = rt2CtrlData.getReadBuffer().deltaT;
-        Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
-        Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
-        Eigen::VectorXf targetVels = dmpCtrl->getTargetVelocity();
-        Eigen::Matrix4f targetPose = dmpCtrl->getIntegratedPoseMat();
-
-        debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
-        debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
-        VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
-        debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
-        debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
-        debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
-        debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
-        debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
-        debugOutputData.getWriteBuffer().mpcFactor =  dmpCtrl->debugData.mpcFactor;
-        debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
-        debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
-        debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
-        debugOutputData.getWriteBuffer().deltaT = deltaT;
-        debugOutputData.commitWrite();
-
-        getWriterControlStruct().targetTSVel = targetVels;
-        getWriterControlStruct().targetPose = targetPose;
-        writeControlStruct();
-
-        dmpRunning = true;
-    }
-
-
-    void NJointPeriodicTSDMPForwardVelController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
-        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
-        rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
-        rt2UserData.commitWrite();
-
-        if (firstRun || !dmpRunning)
-        {
-            targetPose = currentPose;
-            for (size_t i = 0; i < targets.size(); ++i)
-            {
-                targets.at(i)->velocity = 0.0f;
-            }
-            firstRun = false;
-            filteredForce.setZero();
-
-            Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
-            toolTransform = currentHandOri.transpose();
-            // force calibration
-            if (!dmpRunning)
-            {
-                forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
-            }
-        }
-        else
-        {
-
-            Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-            Eigen::VectorXf qvel;
-            qvel.resize(velocitySensors.size());
-            for (size_t i = 0; i < velocitySensors.size(); ++i)
-            {
-                qvel(i) = velocitySensors[i]->velocity;
-            }
-
-            Eigen::VectorXf tcptwist = jacobi * qvel;
-
-            rt2CtrlData.getWriteBuffer().currentPose = currentPose;
-            rt2CtrlData.getWriteBuffer().currentTwist = tcptwist;
-            rt2CtrlData.getWriteBuffer().deltaT = deltaT;
-            rt2CtrlData.getWriteBuffer().currentTime += deltaT;
-            rt2CtrlData.commitWrite();
-
-
-            // forward controller
-            rtUpdateControlStruct();
-            Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
-            //        Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
-
-            // force detection
-            //            filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
-            //            Eigen::Vector3f filteredForceInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->toGlobalCoordinateSystemVec(filteredForce);
-            //            filteredForceInRoot = rtGetRobot()->getRootNode()->toLocalCoordinateSystemVec(filteredForceInRoot);
-            //            float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
-
-            //            Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
-            //            Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri;
-            //            targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
-            //            Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir;
-
-            //            ARMARX_IMPORTANT << "original force: " << forceSensor->force;
-            //            ARMARX_IMPORTANT << "filteredForce: " << filteredForce;
-            //            ARMARX_IMPORTANT << "filteredForceInRoot: " << filteredForceInRoot;
-            //            ARMARX_IMPORTANT << "forceOffset: " << forceOffset;
-            //            ARMARX_IMPORTANT << "currentToolOri: " << currentToolOri;
-
-            for (size_t i = 3; i < 6; ++i)
-            {
-                targetVel(i) = 0;
-            }
-
-            //            float forceCtrl = kpf * (targetForce - filteredForceInRoot.norm());
-            //            Eigen::Vector3f desiredZVel = - forceCtrl * (currentToolDir / currentToolDir.norm());
-            //            targetVel.block<3, 1>(0, 0) += desiredZVel;
-
-            // dead zone for force
-            //        if (filteredForceInRoot.norm() > cfg->minimumReactForce)
-            //        {
-            //            // rotation changes
-            //            Eigen::Vector3f axis = oriToolDir.cross(filteredForceInRoot);
-            //            float angle = oriToolDir.dot(filteredForceInRoot);
-            //            Eigen::AngleAxisf desiredToolOri(angle, axis);
-            //            Eigen::Matrix3f desiredHandOri = toolTransform.transpose() * desiredToolOri;
-            //            Eigen::Matrix3f desiredRotMat = desiredHandOri * currentHandOri.transpose();
-            //            Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
-            //            for (size_t i = 3; i < 6; ++i)
-            //            {
-            //                targetVel(i) = desiredRPY(i - 3);
-            //            }
-            //        }}
-
-            //            ARMARX_IMPORTANT << "targetVel: " << targetVel;
-            //            ARMARX_IMPORTANT << "targetPose: " << targetPose;
-
-            //            targetPose.block<3, 1>(0, 3) += deltaT * targetVel.block<3, 1>(0, 0);
-            //            Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * deltaT, targetVel(4) * deltaT, targetVel(5) * deltaT);
-            //            targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
-
-            float dTf = (float)deltaT;
-            targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
-            Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
-            targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
-
-            ARMARX_IMPORTANT << "targetVel: " <<  targetVel.block<3, 1>(0, 0);
-            ARMARX_IMPORTANT << "targetPose: " << targetPose;
-            ARMARX_IMPORTANT << "deltaT: " << deltaT;
-
-            Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
-            Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-            Eigen::VectorXf rtTargetVel = targetVel;
-            rtTargetVel.block<3, 1>(0, 0) += cfg->Kpos * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3));
-            rtTargetVel.block<3, 1>(3, 0) += cfg->Kori * errorRPY;
-
-            float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
-            if (normLinearVelocity > fabs(cfg->maxLinearVel))
-            {
-                rtTargetVel.block<3, 1>(0, 0) = fabs(cfg->maxLinearVel) * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
-            }
-
-            float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
-            if (normAngularVelocity > fabs(cfg->maxAngularVel))
-            {
-                rtTargetVel.block<3, 1>(3, 0) = fabs(cfg->maxAngularVel) * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
-            }
-
-
-            // cartesian vel controller
-
-            Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
-            if (cfg->avoidJointLimitsKp > 0)
-            {
-                jnv += cfg->avoidJointLimitsKp * tcpController->calculateJointLimitAvoidance();
-            }
-
-            Eigen::VectorXf jointTargetVelocities = tcpController->calculate(rtTargetVel, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
-            for (size_t i = 0; i < targets.size(); ++i)
-            {
-                targets.at(i)->velocity = jointTargetVelocities(i);
-                if (!targets.at(i)->isValid())
-                {
-                    targets.at(i)->velocity = 0.0f;
-                }
-                else
-                {
-                    if (fabs(targets.at(i)->velocity) > fabs(cfg->maxJointVel))
-                    {
-                        targets.at(i)->velocity = fabs(cfg->maxJointVel) * (targets.at(i)->velocity / fabs(targets.at(i)->velocity));
-                    }
-                }
-            }
-        }
-
-    }
-
-
-    void NJointPeriodicTSDMPForwardVelController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        ARMARX_INFO << "Learning DMP ... ";
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->learnDMPFromFiles(fileNames);
-
-    }
-
-    void NJointPeriodicTSDMPForwardVelController::setSpeed(Ice::Double times, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setSpeed(times);
-    }
-
-
-    void NJointPeriodicTSDMPForwardVelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setGoalPoseVec(goals);
-    }
-
-    void NJointPeriodicTSDMPForwardVelController::setAmplitude(Ice::Double amp, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setAmplitude(amp);
-    }
-
-    void NJointPeriodicTSDMPForwardVelController::runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&)
-    {
-        firstRun = true;
-        while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
-        {
-            usleep(100);
-        }
-
-        Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-        dmpCtrl->setSpeed(tau);
-        finished = false;
-
-        ARMARX_INFO << "run DMP";
-        started = true;
-        dmpRunning = false;
-    }
-
-
-    void NJointPeriodicTSDMPForwardVelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-        std::string datafieldName;
-        std::string debugName = "Periodic";
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        for (auto& pair : values)
-        {
-            datafieldName = pair.first  + "_" + debugName;
-            datafields[datafieldName] = new Variant(pair.second);
-        }
-
-        auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
-        for (auto& pair : currentPose)
-        {
-            datafieldName = pair.first + "_" + debugName;
-            datafields[datafieldName] = new Variant(pair.second);
-        }
-
-        datafieldName = "canVal_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        datafieldName = "mpcFactor_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
-        datafieldName = "error_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
-        datafieldName = "posError_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
-        datafieldName = "oriError_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
-        datafieldName = "deltaT_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-        datafieldName = "DMPController_" + debugName;
-
-        debugObs->setDebugChannel(datafieldName, datafields);
-    }
-
-    void NJointPeriodicTSDMPForwardVelController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-
-        RTToControllerData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentPose = tcp->getPoseInRootFrame();
-        initSensorData.currentTwist.setZero();
-        rt2CtrlData.reinitAllBuffers(initSensorData);
-
-        RTToUserData initInterfaceData;
-        initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
-        rt2UserData.reinitAllBuffers(initInterfaceData);
-
-
-        started = false;
-        runTask("NJointPeriodicTSDMPForwardVelController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-
-    }
-
-    void NJointPeriodicTSDMPForwardVelController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp.F24458 b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp.F24458
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h
deleted file mode 100644
index 907bf3866421f512cce5bcfb2e37d79a24db08db..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h
+++ /dev/null
@@ -1,152 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-namespace armarx
-{
-
-
-    TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPForwardVelController);
-    TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPForwardVelControllerControlData);
-
-    class NJointPeriodicTSDMPForwardVelControllerControlData
-    {
-    public:
-        Eigen::VectorXf targetTSVel;
-        Eigen::Matrix4f targetPose;
-    };
-
-    /**
-     * @brief The NJointPeriodicTSDMPForwardVelController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointPeriodicTSDMPForwardVelController :
-        public NJointControllerWithTripleBuffer<NJointPeriodicTSDMPForwardVelControllerControlData>,
-        public NJointPeriodicTSDMPControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointPeriodicTSDMPControllerConfigPtr;
-        NJointPeriodicTSDMPForwardVelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointPeriodicTSDMPForwardVelControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
-        bool isFinished(const Ice::Current&)
-        {
-            return finished;
-        }
-
-        void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&);
-        void setSpeed(Ice::Double times, const Ice::Current&);
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void setAmplitude(Ice::Double amp, const Ice::Current&);
-
-
-        double getCanVal(const Ice::Current&)
-        {
-            return dmpCtrl->canVal;
-        }
-
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitNJointController();
-        void onDisconnectNJointController();
-        void controllerRun();
-
-    private:
-        struct DebugBufferData
-        {
-            StringFloatDictionary latestTargetVelocities;
-            StringFloatDictionary currentPose;
-            double currentCanVal;
-            double mpcFactor;
-            double error;
-            double phaseStop;
-            double posError;
-            double oriError;
-            double deltaT;
-        };
-
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-        struct RTToControllerData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-        };
-        TripleBuffer<RTToControllerData> rt2CtrlData;
-
-        struct RTToUserData
-        {
-            Eigen::Matrix4f currentTcpPose;
-            float waitTimeForCalibration;
-        };
-        TripleBuffer<RTToUserData> rt2UserData;
-
-
-        TaskSpaceDMPControllerPtr dmpCtrl;
-
-        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-        std::vector<ControlTarget1DoFActuatorVelocity*> targets;
-
-        // velocity ik controller parameters
-        CartesianVelocityControllerPtr tcpController;
-        std::string nodeSetName;
-
-        // dmp parameters
-        bool finished;
-        bool started;
-        bool dmpRunning;
-        bool firstRun;
-
-        VirtualRobot::DifferentialIKPtr ik;
-        VirtualRobot::RobotNodePtr tcp;
-
-        NJointPeriodicTSDMPControllerConfigPtr cfg;
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointPeriodicTSDMPForwardVelController>::pointer_type controllerTask;
-
-        Eigen::Matrix4f targetPose;
-
-
-        const SensorValueForceTorque* forceSensor;
-        Eigen::Vector3f filteredForce;
-        Eigen::Vector3f forceOffset;
-        Eigen::Matrix3f toolTransform;
-        Eigen::Vector3f oriToolDir;
-        struct UserToRTData
-        {
-            float targetForce;
-        };
-        TripleBuffer<UserToRTData> user2rtData;
-        float kpf;
-
-        // NJointPeriodicTSDMPControllerInterface interface
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
deleted file mode 100644
index 30bc22bd2267654044f698cbb2e17225432cd8ee..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ /dev/null
@@ -1,634 +0,0 @@
-#include "NJointTSDMPController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-#include <boost/archive/text_oarchive.hpp>
-#include <boost/archive/text_iarchive.hpp>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointTSDMPController> registrationControllerNJointTSDMPController("NJointTSDMPController");
-
-    NJointTSDMPController::NJointTSDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        useSynchronizedRtRobot();
-        cfg = NJointTaskSpaceDMPControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
-        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-        jointNames = rns->getNodeNames();
-        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-        for (size_t i = 0; i < rns->getSize(); ++i)
-        {
-            std::string jointName = rns->getNode(i)->getName();
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-
-            const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-            if (!torqueSensor)
-            {
-                ARMARX_WARNING << "No Torque sensor available for " << jointName;
-            }
-            if (!gravityTorqueSensor)
-            {
-                ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
-            }
-
-            torqueSensors.push_back(torqueSensor);
-            gravityTorqueSensors.push_back(gravityTorqueSensor);
-            velocitySensors.push_back(velocitySensor);
-            positionSensors.push_back(positionSensor);
-        };
-
-        tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
-        refFrame = (cfg->frameName.empty()) ? rns->getRobot()->getRootNode() : rtGetRobot()->getRobotNode(cfg->frameName);
-        ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName;
-
-        // set tcp controller
-        tcpController.reset(new CartesianVelocityController(rns, tcp));
-        nodeSetName = cfg->nodeSetName;
-        torquePIDs.resize(tcpController->rns->getSize(), pidController());
-
-        ik.reset(new VirtualRobot::DifferentialIK(rns, refFrame, VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-        finished = false;
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpStyle;
-        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-        dmpCtrl.reset(new TaskSpaceDMPController("default", taskSpaceDMPConfig, false));
-
-        // initialize tcp position and orientation
-
-
-        RTToControllerData initSensorData;
-        initSensorData.deltaT = 0;
-        initSensorData.currentTime = 0;
-        initSensorData.currentPose.setZero();
-        initSensorData.currentTwist.setZero();
-        rt2CtrlData.reinitAllBuffers(initSensorData);
-
-        targetVels.setZero(6);
-        NJointTSDMPControllerControlData initData;
-        initData.targetTSVel.setZero(6);
-        initData.targetPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
-        initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
-        initData.torqueKp.resize(tcpController->rns->getSize(), 0);
-        initData.torqueKd.resize(tcpController->rns->getSize(), 0);
-        initData.mode = ModeFromIce(cfg->mode);
-        reinitTripleBuffer(initData);
-
-        debugName = cfg->debugName;
-
-        KpF = cfg->Kp_LinearVel;
-        KoF = cfg->Kp_AngularVel;
-        DpF = cfg->Kd_LinearVel;
-        DoF = cfg->Kd_AngularVel;
-
-        filtered_qvel.setZero(targets.size());
-        vel_filter_factor = cfg->vel_filter;
-
-        filtered_position.setZero(3);
-        pos_filter_factor = cfg->pos_filter;
-
-        //        jlhigh = rns->getNode("..")->getJointLimitHi();
-        //        jllow = rns->getNode("")->getJointLimitLo();
-        firstRun = true;
-
-        jointLowLimits.setZero(targets.size());
-        jointHighLimits.setZero(targets.size());
-        for (size_t i = 0; i < rns->getSize(); i++)
-        {
-            VirtualRobot::RobotNodePtr rn = rns->getAllRobotNodes().at(i);
-
-            jointLowLimits(i) = rn->getJointLimitLo();
-            jointHighLimits(i) = rn->getJointLimitHi();
-        }
-
-        started = false;
-
-        RTToUserData initInterfaceData;
-        initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
-        rt2UserData.reinitAllBuffers(initInterfaceData);
-    }
-
-    std::string NJointTSDMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointTSDMPController";
-    }
-
-    void NJointTSDMPController::controllerRun()
-    {
-        if (!started)
-        {
-            return;
-        }
-
-        if (!rt2CtrlData.updateReadBuffer() || !dmpCtrl)
-        {
-            return;
-        }
-
-        double deltaT = rt2CtrlData.getReadBuffer().deltaT;
-        Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
-        Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
-        if (dmpCtrl->canVal < 1e-8)
-        {
-            finished = true;
-        }
-        targetVels = dmpCtrl->getTargetVelocity();
-        targetPose = dmpCtrl->getTargetPoseMat();
-        std::vector<double> targetState = dmpCtrl->getTargetPose();
-
-        debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
-        debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
-        debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
-        debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
-
-        VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
-        debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
-        debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
-        debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
-        debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
-        debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
-        debugOutputData.getWriteBuffer().mpcFactor =  dmpCtrl->debugData.mpcFactor;
-        debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
-        debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
-        debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
-        debugOutputData.getWriteBuffer().deltaT = deltaT;
-
-        debugOutputData.commitWrite();
-
-        getWriterControlStruct().targetTSVel = targetVels;
-        getWriterControlStruct().targetPose = targetPose;
-        writeControlStruct();
-    }
-
-    Eigen::VectorXf NJointTSDMPController::calcIK(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel,  VirtualRobot::IKSolver::CartesianSelection mode)
-    {
-        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, mode);
-
-        Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
-
-        Eigen::MatrixXf nullspace = lu_decomp.kernel();
-        Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
-        for (int i = 0; i < nullspace.cols(); i++)
-        {
-            float squaredNorm = nullspace.col(i).squaredNorm();
-            // Prevent division by zero
-            if (squaredNorm > 1.0e-32f)
-            {
-                nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
-            }
-        }
-
-        Eigen::MatrixXf inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
-        //        ARMARX_INFO << "inv: " << inv;
-        Eigen::VectorXf jointVel = inv * cartesianVel;
-        //        jointVel += nsv;
-        return jointVel;
-    }
-
-    void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        Eigen::Matrix4f currentPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
-        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
-        rt2UserData.commitWrite();
-
-        if (firstRun)
-        {
-            filtered_position = currentPose.block<3, 1>(0, 3);
-
-            firstRun = false;
-            for (size_t i = 0; i < targets.size(); ++i)
-            {
-                targets.at(i)->velocity = 0;
-            }
-            return;
-        }
-        else
-        {
-            filtered_position = (1 - pos_filter_factor) * filtered_position + pos_filter_factor * currentPose.block<3, 1>(0, 3);
-        }
-
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf qvel(velocitySensors.size());
-        for (size_t i = 0; i < velocitySensors.size(); ++i)
-        {
-            qvel(i) = velocitySensors[i]->velocity;
-        }
-
-        filtered_qvel = (1 - vel_filter_factor) * filtered_qvel + vel_filter_factor * qvel;
-        Eigen::VectorXf tcptwist = jacobi * filtered_qvel;
-
-        rt2CtrlData.getWriteBuffer().currentPose = currentPose;
-        rt2CtrlData.getWriteBuffer().currentTwist = tcptwist;
-        rt2CtrlData.getWriteBuffer().deltaT = deltaT;
-        rt2CtrlData.getWriteBuffer().currentTime += deltaT;
-        rt2CtrlData.commitWrite();
-
-        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
-        rt2UserData.commitWrite();
-
-        Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
-        Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
-
-        Eigen::VectorXf jointTargetVelocities = Eigen::VectorXf::Zero(targets.size());
-        if (started)
-        {
-            //            targetVel = rtGetControlStruct().targetTSVel;
-            //            targetPose = rtGetControlStruct().targetPose;
-
-            Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
-            Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
-            Eigen::Vector6f rtTargetVel;
-            rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) + DpF * (- tcptwist.block<3, 1>(0, 0));
-            //        rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) + DpF * (targetVel.block<3, 1>(0, 0) - tcptwist.block<3, 1>(0, 0));
-            rtTargetVel.block<3, 1>(3, 0) = KoF * errorRPY + DoF * (targetVel.block<3, 1>(3, 0) - tcptwist.block<3, 1>(3, 0));
-            //        rtTargetVel = targetVel;
-
-
-
-            float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
-            if (normLinearVelocity > cfg->maxLinearVel)
-            {
-                rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
-            }
-
-            float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
-            if (normAngularVelocity > cfg->maxAngularVel)
-            {
-                rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
-            }
-
-
-            // cartesian vel controller
-            //            Eigen::Vector6f x;
-            //            for (size_t i = 0; i < 6; i++)
-            //            {
-            //                x(i) = rtTargetVel(i);
-            //            }
-
-            Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
-            float jointLimitAvoidanceKp = rtGetControlStruct().avoidJointLimitsKp;
-            if (jointLimitAvoidanceKp > 0)
-            {
-                jnv += jointLimitAvoidanceKp * tcpController->calculateJointLimitAvoidance();
-            }
-            for (size_t i = 0; i < tcpController->rns->getSize(); i++)
-            {
-                jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
-            }
-
-            //            jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
-            jointTargetVelocities = calcIK(rtTargetVel, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
-            // Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
-            ARMARX_CHECK_EXPRESSION(!targets.empty());
-            ARMARX_CHECK_LESS(targets.size(), 1000);
-        }
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            targets.at(i)->velocity = jointTargetVelocities(i);
-
-            if (!targets.at(i)->isValid() || fabs(targets.at(i)->velocity) > cfg->maxJointVelocity)
-            {
-                targets.at(i)->velocity = 0.0f;
-
-            }
-        }
-        rtDebugData.getWriteBuffer().targetJointVels = jointTargetVelocities;
-        rtDebugData.commitWrite();
-    }
-
-
-    void NJointTSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        ARMARX_INFO << "Learning DMP ... ";
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->learnDMPFromFiles(fileNames);
-    }
-
-    void NJointTSDMPController::setSpeed(Ice::Double times, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setSpeed(times);
-    }
-
-    void NJointTSDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setViaPose(u, viapoint);
-    }
-
-    void NJointTSDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
-    {
-        LockGuardType guard {controlDataMutex};
-        for (size_t i = 0; i < tcpController->rns->getSize(); i++)
-        {
-            getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
-        }
-        writeControlStruct();
-    }
-
-    void NJointTSDMPController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
-    {
-        LockGuardType guard {controlDataMutex};
-        for (size_t i = 0; i < tcpController->rns->getSize(); i++)
-        {
-            getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
-        }
-        writeControlStruct();
-    }
-
-    void NJointTSDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
-    {
-        LockGuardType guard {controlDataMutex};
-        getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
-        getWriterControlStruct().mode = ModeFromIce(mode);
-        writeControlStruct();
-    }
-
-
-    void NJointTSDMPController::removeAllViaPoints(const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        ARMARX_INFO << "setting via points ";
-        dmpCtrl->removeAllViaPoints();
-    }
-
-
-    void NJointTSDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->setGoalPoseVec(goals);
-    }
-
-    void NJointTSDMPController::pauseDMP(const Ice::Current&)
-    {
-        dmpCtrl->pauseController();
-    }
-
-    void NJointTSDMPController::resumeDMP(const Ice::Current&)
-    {
-        dmpCtrl->resumeController();
-    }
-
-    void NJointTSDMPController::resetDMP(const Ice::Current&)
-    {
-        if (started)
-        {
-            ARMARX_INFO << "Cannot reset running DMP";
-        }
-        firstRun = true;
-    }
-
-    void NJointTSDMPController::stopDMP(const Ice::Current&)
-    {
-        started = false;
-    }
-
-    std::string NJointTSDMPController::getDMPAsString(const Ice::Current&)
-    {
-
-        return dmpCtrl->saveDMPToString();
-    }
-
-    std::vector<double> NJointTSDMPController::createDMPFromString(const std::string& dmpString, const Ice::Current&)
-    {
-        dmpCtrl->loadDMPFromString(dmpString);
-        return dmpCtrl->dmpPtr->defaultGoal;
-    }
-
-    VirtualRobot::IKSolver::CartesianSelection NJointTSDMPController::ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
-    {
-        if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
-        {
-            return VirtualRobot::IKSolver::CartesianSelection::Position;
-        }
-        if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
-        {
-            return VirtualRobot::IKSolver::CartesianSelection::Orientation;
-        }
-        if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
-        {
-            return VirtualRobot::IKSolver::CartesianSelection::All;
-        }
-        ARMARX_ERROR_S << "invalid mode " << mode;
-        return (VirtualRobot::IKSolver::CartesianSelection)0;
-    }
-
-
-
-    void NJointTSDMPController::runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&)
-    {
-        ARMARX_INFO << "------dmp controller: " << VAROUT(goals);
-        firstRun = true;
-        while (firstRun)
-        {
-            usleep(100);
-        }
-        while (!rt2UserData.updateReadBuffer())
-        {
-            usleep(100);
-        }
-
-        Eigen::Matrix4f pose = rt2UserData.getReadBuffer().currentTcpPose;
-
-        LockGuardType guard {controllerMutex};
-        //        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-        finished = false;
-
-        ARMARX_INFO << "run DMP";
-        started = true;
-    }
-
-    void NJointTSDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
-    {
-        firstRun = true;
-        while (firstRun)
-        {
-            usleep(100);
-        }
-        while (!rt2UserData.updateReadBuffer())
-        {
-            usleep(100);
-        }
-
-        Eigen::Matrix4f pose = rt2UserData.getReadBuffer().currentTcpPose;
-
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->config.motionTimeDuration = timeDuration;
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
-        finished = false;
-        started = true;
-    }
-
-
-    void NJointTSDMPController::rtPreActivateController()
-    {
-    }
-
-    void NJointTSDMPController::rtPostDeactivateController()
-    {
-
-    }
-
-    void NJointTSDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-        std::string datafieldName = debugName;
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        for (auto& pair : values)
-        {
-            datafieldName = pair.first  + "_" + debugName;
-            datafields[datafieldName] = new Variant(pair.second);
-        }
-
-        auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
-        for (auto& pair : dmpTargets)
-        {
-            datafieldName = pair.first  + "_" + debugName;
-            datafields[datafieldName] = new Variant(pair.second);
-        }
-
-        auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
-        for (auto& pair : currentPose)
-        {
-            datafieldName = pair.first + "_" + debugName;
-            datafields[datafieldName] = new Variant(pair.second);
-        }
-
-        datafieldName = "canVal_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        datafieldName = "mpcFactor_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
-        datafieldName = "error_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
-        datafieldName = "posError_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
-        datafieldName = "oriError_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
-        datafieldName = "deltaT_" + debugName;
-        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-
-
-        Eigen::VectorXf targetJoints = rtDebugData.getUpToDateReadBuffer().targetJointVels;
-        for (int i = 0; i < targetJoints.size(); ++i)
-        {
-            datafieldName = jointNames[i] + "_velocity";
-            datafields[datafieldName] = new Variant(targetJoints[i]);
-        }
-
-        datafieldName = "DMPController_" + debugName;
-        debugObs->setDebugChannel(datafieldName, datafields);
-    }
-
-    void NJointTSDMPController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        started = false;
-        runTask("NJointTSDMPController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-
-    }
-
-    void NJointTSDMPController::onDisconnectNJointController()
-    {
-        ARMARX_INFO << "stopped ...";
-    }
-
-    void NJointTSDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
-    {
-        dmpCtrl->setWeights(weights);
-    }
-
-    DoubleSeqSeq NJointTSDMPController::getMPWeights(const Ice::Current&)
-    {
-        DMP::DVec2d res = dmpCtrl->getWeights();
-        DoubleSeqSeq resvec;
-        for (size_t i = 0; i < res.size(); ++i)
-        {
-            std::vector<double> cvec;
-            for (size_t j = 0; j < res[i].size(); ++j)
-            {
-                cvec.push_back(res[i][j]);
-            }
-            resvec.push_back(cvec);
-        }
-
-        return resvec;
-    }
-
-    void NJointTSDMPController::setLinearVelocityKd(Ice::Float kd, const Ice::Current&)
-    {
-        DpF = kd;
-    }
-
-    void NJointTSDMPController::setLinearVelocityKp(Ice::Float kp, const Ice::Current&)
-    {
-        KpF = kp;
-    }
-
-    void NJointTSDMPController::setAngularVelocityKd(Ice::Float kd, const Ice::Current&)
-    {
-        DoF = kd;
-    }
-
-    void NJointTSDMPController::setAngularVelocityKp(Ice::Float kp, const Ice::Current&)
-    {
-        KoF = kp;
-    }
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
deleted file mode 100644
index 49e0fd68dfd4dfd0238e6f2e4c365af66e72e4c4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ /dev/null
@@ -1,217 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-namespace armarx
-{
-    TYPEDEF_PTRS_HANDLE(NJointTSDMPController);
-    TYPEDEF_PTRS_HANDLE(NJointTSDMPControllerControlData);
-
-    using ViaPoint = std::pair<double, DMP::DVec >;
-    using ViaPointsSet = std::vector<ViaPoint >;
-    class NJointTSDMPControllerControlData
-    {
-    public:
-        Eigen::Vector6f targetTSVel;
-        Eigen::Matrix4f targetPose;
-        // cartesian velocity control data
-        std::vector<float> nullspaceJointVelocities;
-        float avoidJointLimitsKp = 0;
-        std::vector<float> torqueKp;
-        std::vector<float> torqueKd;
-        VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
-    };
-
-    /**
-     * @brief The NJointTSDMPController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointTSDMPController :
-        public NJointControllerWithTripleBuffer<NJointTSDMPControllerControlData>,
-        public NJointTaskSpaceDMPControllerInterface
-    {
-        class pidController
-        {
-        public:
-            float Kp = 0, Kd = 0;
-            float lastError = 0;
-            float update(float dt, float error)
-            {
-                float derivative = (error - lastError) / dt;
-                float retVal = Kp * error + Kd * derivative;
-                lastError = error;
-                return retVal;
-            }
-        };
-    public:
-        using ConfigPtrT = NJointTaskSpaceDMPControllerConfigPtr;
-        NJointTSDMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const override;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        // NJointTSDMPControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
-        bool isFinished(const Ice::Current&) override
-        {
-            return finished;
-        }
-
-        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&) override;
-        void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
-
-        void setSpeed(Ice::Double times, const Ice::Current&) override;
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
-        void removeAllViaPoints(const Ice::Current&) override;
-
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
-        void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&) override;
-        void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
-        void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
-
-
-        void pauseDMP(const Ice::Current&) override;
-        void resumeDMP(const Ice::Current&) override;
-
-        void resetDMP(const Ice::Current&) override;
-        void stopDMP(const Ice::Current&) override;
-
-        double getCanVal(const Ice::Current&) override
-        {
-            return dmpCtrl->canVal;
-        }
-        std::string getDMPAsString(const Ice::Current&) override;
-        std::vector<double> createDMPFromString(const std::string& dmpString, const Ice::Current&) override;
-
-        // VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
-        Eigen::VectorXf calcIK(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspace, VirtualRobot::IKSolver::CartesianSelection mode);
-
-
-        void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
-        DoubleSeqSeq getMPWeights(const Ice::Current&) override;
-
-        void setLinearVelocityKd(Ice::Float kd, const Ice::Current& = Ice::emptyCurrent) override;
-        void setLinearVelocityKp(Ice::Float kp, const Ice::Current& = Ice::emptyCurrent) override;
-        void setAngularVelocityKd(Ice::Float kd, const Ice::Current& = Ice::emptyCurrent) override;
-        void setAngularVelocityKp(Ice::Float kp, const Ice::Current& = Ice::emptyCurrent) override;
-
-    protected:
-        void rtPreActivateController() override;
-        void rtPostDeactivateController() override;
-        VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
-        void onInitNJointController() override;
-        void onDisconnectNJointController() override;
-        void controllerRun();
-
-    private:
-        std::vector<std::string> jointNames;
-        struct DebugBufferData
-        {
-            StringFloatDictionary latestTargetVelocities;
-            StringFloatDictionary dmpTargets;
-            StringFloatDictionary currentPose;
-            double currentCanVal;
-            double mpcFactor;
-            double error;
-            double phaseStop;
-            double posError;
-            double oriError;
-            double deltaT;
-        };
-
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-
-        struct RTDebugData
-        {
-            Eigen::VectorXf targetJointVels;
-        };
-
-        TripleBuffer<RTDebugData> rtDebugData;
-
-
-        struct RTToControllerData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-        };
-        TripleBuffer<RTToControllerData> rt2CtrlData;
-
-        struct RTToUserData
-        {
-            Eigen::Matrix4f currentTcpPose;
-
-        };
-        TripleBuffer<RTToUserData> rt2UserData;
-
-
-        TaskSpaceDMPControllerPtr dmpCtrl;
-
-        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
-        std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
-        std::vector<ControlTarget1DoFActuatorVelocity*> targets;
-        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-
-        // velocity ik controller parameters
-        std::vector<pidController> torquePIDs;
-        CartesianVelocityControllerPtr tcpController;
-        std::string nodeSetName;
-
-        // dmp parameters
-        bool finished;
-        bool started;
-
-        VirtualRobot::DifferentialIKPtr ik;
-        VirtualRobot::RobotNodePtr tcp;
-        VirtualRobot::RobotNodePtr refFrame;
-        Eigen::Vector6f targetVels;
-        Eigen::Matrix4f targetPose;
-
-        NJointTaskSpaceDMPControllerConfigPtr cfg;
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointTSDMPController>::pointer_type controllerTask;
-
-
-        std::string debugName;
-
-        Eigen::VectorXf filtered_qvel;
-        Eigen::Vector3f filtered_position;
-        float vel_filter_factor;
-        float pos_filter_factor;
-        bool firstRun;
-
-        float KpF;
-        float DpF;
-        float KoF;
-        float DoF;
-
-        Eigen::VectorXf jointHighLimits;
-        Eigen::VectorXf jointLowLimits;
-
-
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp
deleted file mode 100644
index a180c4782e88c9a1fcba2cfccc932163b6ffb3f7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp
+++ /dev/null
@@ -1,761 +0,0 @@
-#include "NJointTaskSpaceAdaptiveDMPController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointTaskSpaceAdaptiveDMPController> registrationControllerNJointTaskSpaceAdaptiveDMPController("NJointTaskSpaceAdaptiveDMPController");
-
-    NJointTaskSpaceAdaptiveDMPController::NJointTaskSpaceAdaptiveDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_INFO << "creating impedance dmp controller";
-        cfg = NJointTaskSpaceAdaptiveDMPControllerConfigPtr::dynamicCast(config);
-        useSynchronizedRtRobot();
-        rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-
-        for (size_t i = 0; i < rns->getSize(); ++i)
-        {
-            std::string jointName = rns->getNode(i)->getName();
-            jointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-            velocitySensors.push_back(velocitySensor);
-            positionSensors.push_back(positionSensor);
-        };
-
-
-        const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
-        forceSensor = svlf->asA<SensorValueForceTorque>();
-
-        tcp =  rns->getTCP();
-        ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        numOfJoints = targets.size();
-        // set DMP
-        double phaseL = 1000;
-        double phaseK = 1000;
-        double phaseDist0 = 10000;
-        double phaseDist1 = 10000;
-        double posToOriRatio = 10;
-
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
-        taskSpaceDMPConfig.DMPAmplitude = 1.0;
-        taskSpaceDMPConfig.phaseStopParas.goDist = phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Kori = 0;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = phaseK;
-
-        dmpCtrl.reset(new TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
-        finished = false;
-
-        useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
-        nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
-
-        isNullSpaceJointDMPLearned = false;
-
-        defaultNullSpaceJointValues.resize(targets.size());
-        ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            defaultNullSpaceJointValues(i) = cfg->defaultNullSpaceJointValues.at(i);
-        }
-
-
-        kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
-        dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
-        kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
-        dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
-
-        ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
-        ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
-
-        knull.setZero(targets.size());
-        dnull.setZero(targets.size());
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            knull(i) = cfg->Knull.at(i);
-            dnull(i) = cfg->Dnull.at(i);
-        }
-
-        torqueLimit = cfg->torqueLimit;
-        timeDuration = cfg->timeDuration;
-
-        NJointTaskSpaceAdaptiveDMPControllerInterfaceData initInterfaceData;
-        initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
-        initInterfaceData.currentForce = Eigen::Vector3f::Zero();
-        initInterfaceData.currentVel.setZero(6);
-        interfaceData.reinitAllBuffers(initInterfaceData);
-
-        NJointTaskSpaceAdaptiveDMPControllerSensorData initControllerSensorData;
-        initControllerSensorData.currentPose = Eigen::Matrix4f::Identity();
-        initControllerSensorData.currentTime = 0;
-        initControllerSensorData.deltaT = 0;
-        initControllerSensorData.currentTwist.setZero();
-        controllerSensorData.reinitAllBuffers(initControllerSensorData);
-
-        //initialize control parameters
-        Eigen::VectorXf KpImpedance;
-        KpImpedance.setZero(6);
-
-        for (int i = 0; i < 3; ++i)
-        {
-            KpImpedance(i) = cfg->Kpos.at(i);
-            KpImpedance(i + 3) = cfg->Kori.at(i);
-        }
-
-        Eigen::VectorXf KdImpedance;
-        KdImpedance.setZero(6);
-
-        for (int i = 0; i < 3; ++i)
-        {
-            KdImpedance(i) = cfg->Dpos.at(i);
-            KdImpedance(i + 3) = cfg->Dori.at(i);
-
-        }
-
-        Inferface2rtData initInt2rtData;
-        initInt2rtData.KpImpedance = KpImpedance;
-        initInt2rtData.KdImpedance = KdImpedance;
-        initInt2rtData.Knull = knull;
-        initInt2rtData.Dnull = dnull;
-        interface2rtBuffer.reinitAllBuffers(initInt2rtData);
-
-
-        Interface2CtrlData initInt2CtrlData;
-        initInt2CtrlData.canVal = 1.0;
-        interface2CtrlBuffer.reinitAllBuffers(initInt2CtrlData);
-
-        firstRun = true;
-        forceOffset.setZero(3);
-        filteredForce.setZero(3);
-
-        ARMARX_INFO << "Finished controller constructor ";
-    }
-
-    std::string NJointTaskSpaceAdaptiveDMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointTaskSpaceAdaptiveDMPController";
-
-    }
-
-
-    void NJointTaskSpaceAdaptiveDMPController::rtPreActivateController()
-    {
-        NJointTaskSpaceAdaptiveDMPControllerControlData initData;
-        initData.targetPose = tcp->getPoseInRootFrame();
-        initData.targetVel.resize(6);
-        initData.targetVel.setZero();
-        initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues;
-        reinitTripleBuffer(initData);
-
-
-    }
-
-
-    void NJointTaskSpaceAdaptiveDMPController::controllerRun()
-    {
-
-
-        if (!dmpCtrl)
-        {
-            return;
-        }
-
-        if (!controllerSensorData.updateReadBuffer())
-        {
-            return;
-        }
-
-
-        double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
-        Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
-        Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
-
-        if (interface2CtrlBuffer.updateReadBuffer())
-        {
-            dmpCtrl->canVal = interface2CtrlBuffer.getUpToDateReadBuffer().canVal;
-        }
-
-        if (!started)
-        {
-            LockGuardType guard {controlDataMutex};
-            getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
-            getWriterControlStruct().targetVel.setZero(6);
-            getWriterControlStruct().targetPose = currentPose;
-            getWriterControlStruct().canVal = 1.0;
-            getWriterControlStruct().mpcFactor = 0.0;
-            writeControlStruct();
-        }
-        else
-        {
-            if (stopped)
-            {
-
-                LockGuardType guard {controlDataMutex};
-                getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
-                getWriterControlStruct().targetVel.setZero(6);
-                getWriterControlStruct().targetPose = oldPose;
-                getWriterControlStruct().canVal = dmpCtrl->canVal;
-                getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
-                writeControlStruct();
-            }
-            else
-            {
-                if (dmpCtrl->canVal < 1e-8)
-                {
-                    finished = true;
-                    LockGuardType guard {controlDataMutex};
-                    getWriterControlStruct().targetVel.setZero();
-                    writeControlStruct();
-                    return;
-                }
-
-                dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
-                Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
-                if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
-                {
-                    DMP::DVec targetJointState;
-                    currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState, dmpCtrl->canVal / timeDuration, deltaT / timeDuration, targetJointState);
-
-                    if (targetJointState.size() == jointNames.size())
-                    {
-                        for (size_t i = 0; i < targetJointState.size(); ++i)
-                        {
-                            desiredNullSpaceJointValues(i) = targetJointState[i];
-                        }
-                    }
-                    else
-                    {
-                        desiredNullSpaceJointValues = defaultNullSpaceJointValues;
-                    }
-                }
-                else
-                {
-                    desiredNullSpaceJointValues = defaultNullSpaceJointValues;
-                }
-
-                LockGuardType guard {controlDataMutex};
-                getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
-                getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
-                getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
-                getWriterControlStruct().canVal = dmpCtrl->canVal;
-                getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
-
-                writeControlStruct();
-            }
-        }
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    {
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
-
-        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf qpos(positionSensors.size());
-        Eigen::VectorXf qvel(velocitySensors.size());
-        for (size_t i = 0; i < positionSensors.size(); ++i)
-        {
-            qpos(i) = positionSensors[i]->position;
-            qvel(i) = velocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf currentTwist = jacobi * qvel;
-
-        controllerSensorData.getWriteBuffer().currentPose = currentPose;
-        controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
-        controllerSensorData.getWriteBuffer().deltaT = deltaT;
-        controllerSensorData.getWriteBuffer().currentTime += deltaT;
-        controllerSensorData.commitWrite();
-
-
-        Eigen::Matrix4f targetPose;
-        Eigen::VectorXf targetVel;
-        Eigen::VectorXf desiredNullSpaceJointValues;
-        if (firstRun || !started)
-        {
-            firstRun = false;
-            targetPose = currentPose;
-            targetVel.setZero(6);
-            desiredNullSpaceJointValues = defaultNullSpaceJointValues;
-            forceOffset = 0.1 * forceOffset + 0.9 * forceSensor->force;
-        }
-        else
-        {
-            filteredForce = (1 - cfg->filterCoeff) * filteredForce + cfg->filterCoeff * (forceSensor->force - forceOffset);
-            targetPose = rtGetControlStruct().targetPose;
-            targetVel = rtGetControlStruct().targetVel;
-            desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
-        }
-
-        interfaceData.getWriteBuffer().currentTcpPose = currentPose;
-        interfaceData.getWriteBuffer().currentForce = filteredForce;
-        interfaceData.getWriteBuffer().currentVel = currentTwist;
-        interfaceData.commitWrite();
-
-
-        kpos = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.head(3);
-        kori = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.tail(3);
-        dpos = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.head(3);
-        dori = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.tail(3);
-        knull = interface2rtBuffer.getUpToDateReadBuffer().Knull;
-        dnull = interface2rtBuffer.getUpToDateReadBuffer().Dnull;
-
-        /* calculate torques in meter */
-        jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
-        Eigen::Vector6f jointControlWrench;
-        {
-            Eigen::Vector3f targetTCPLinearVelocity;
-            targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
-            Eigen::Vector3f currentTCPLinearVelocity;
-            currentTCPLinearVelocity << 0.001 * currentTwist(0),  0.001 * currentTwist(1),  0.001 * currentTwist(2);
-            Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
-            Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
-            Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-
-            Eigen::Vector3f currentTCPAngularVelocity;
-            currentTCPAngularVelocity << currentTwist(3),   currentTwist(4),  currentTwist(5);
-            Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
-            Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-            Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
-            jointControlWrench <<  tcpDesiredForce, tcpDesiredTorque;
-        }
-
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
-
-        Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
-        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
-        Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
-        // torque limit
-        ARMARX_CHECK_EXPRESSION(!targets.empty());
-        ARMARX_CHECK_LESS(targets.size(), 1000);
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            float desiredTorque = jointDesiredTorques(i);
-
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-
-            desiredTorque = (desiredTorque >  torqueLimit) ? torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
-            debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
-            debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] = desiredNullSpaceJointValues(i);
-
-            targets.at(i)->torque = desiredTorque;
-            if (!targets.at(i)->isValid())
-            {
-                ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: " << targets.at(i)->torque;
-                targets.at(i)->torque = 0.0f;
-            }
-        }
-
-
-        debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
-        debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
-        debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
-        debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
-        debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
-        debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
-
-        debugOutputData.getWriteBuffer().impedanceKp_x = kpos(0);
-        debugOutputData.getWriteBuffer().impedanceKp_y = kpos(1);
-        debugOutputData.getWriteBuffer().impedanceKp_z = kpos(2);
-        debugOutputData.getWriteBuffer().impedanceKp_rx = kori(0);
-        debugOutputData.getWriteBuffer().impedanceKp_ry = kori(1);
-        debugOutputData.getWriteBuffer().impedanceKp_rz = kori(2);
-
-        debugOutputData.getWriteBuffer().forceInRoot_x = filteredForce(0);
-        debugOutputData.getWriteBuffer().forceInRoot_y = filteredForce(1);
-        debugOutputData.getWriteBuffer().forceInRoot_z = filteredForce(2);
-        //        debugOutputData.getWriteBuffer().torqueInRoot_x = filteredForce(3);
-        //        debugOutputData.getWriteBuffer().torqueInRoot_y = filteredForce(4);
-        //        debugOutputData.getWriteBuffer().torqueInRoot_z = filteredForce(5);
-
-        debugOutputData.getWriteBuffer().vel_x = currentTwist(0);
-        debugOutputData.getWriteBuffer().vel_y = currentTwist(1);
-        debugOutputData.getWriteBuffer().vel_z = currentTwist(2);
-
-        //        debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
-        //        debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
-
-        debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
-        debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
-        debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
-        VirtualRobot::MathTools::Quaternion targetQuat = VirtualRobot::MathTools::eigen4f2quat(targetPose);
-        debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
-        debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
-        debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
-        debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
-
-        debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
-        VirtualRobot::MathTools::Quaternion currentQuat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
-        debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
-        debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
-        debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
-        debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
-        debugOutputData.getWriteBuffer().deltaT = deltaT;
-
-        debugOutputData.commitWrite();
-
-    }
-
-
-    void NJointTaskSpaceAdaptiveDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        dmpCtrl->learnDMPFromFiles(fileNames);
-        ARMARX_INFO << "Learned DMP ... ";
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-        ARMARX_INFO << "setting via points ";
-        dmpCtrl->setViaPose(u, viapoint);
-
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        dmpCtrl->setGoalPoseVec(goals);
-
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::setCanVal(double canVal, const Ice::Current&)
-    {
-        LockGuardType guard(int2ctrlMutex);
-        interface2CtrlBuffer.getWriteBuffer().canVal = canVal;
-        interface2CtrlBuffer.commitWrite();
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::setUseNullSpaceJointDMP(bool useJointDMP, const Ice::Current&)
-    {
-        useNullSpaceJointDMP = useJointDMP;
-    }
-
-
-    void NJointTaskSpaceAdaptiveDMPController::setDefaultJointValues(const Ice::FloatSeq& desiredJointVals, const Ice::Current&)
-    {
-        ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            defaultNullSpaceJointValues(i) = desiredJointVals.at(i);
-        }
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&)
-    {
-        DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-        DMP::DVec ratios;
-        DMP::SampledTrajectoryV2 traj;
-        traj.readFromCSVFile(fileName);
-        traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
-        if (traj.dim() != jointNames.size())
-        {
-            isNullSpaceJointDMPLearned = false;
-            return;
-        }
-
-        DMP::DVec goal;
-        goal.resize(traj.dim());
-        currentJointState.resize(traj.dim());
-
-        for (size_t i = 0; i < goal.size(); ++i)
-        {
-            goal.at(i) = traj.rbegin()->getPosition(i);
-            currentJointState.at(i).pos = currentJVS.at(i);
-            currentJointState.at(i).vel = 0;
-        }
-
-        trajs.push_back(traj);
-        nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
-
-        // prepare exeuction of joint dmp
-        nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
-        ARMARX_INFO << "prepared nullspace joint dmp";
-        isNullSpaceJointDMPLearned = true;
-    }
-
-
-    void NJointTaskSpaceAdaptiveDMPController::resetDMP(const Ice::Current&)
-    {
-        if (started)
-        {
-            ARMARX_INFO << "Cannot reset running DMP";
-        }
-        firstRun = true;
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
-        interface2rtBuffer.commitWrite();
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), 6);
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
-        interface2rtBuffer.commitWrite();
-
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::setKpNull(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), rns->getSize());
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().Knull = setpoint;
-        interface2rtBuffer.commitWrite();
-
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::setKdNull(const Ice::FloatSeq& value, const Ice::Current&)
-    {
-        Eigen::VectorXf setpoint;
-        setpoint.setZero(value.size());
-        ARMARX_CHECK_EQUAL(value.size(), rns->getSize());
-
-        for (size_t i = 0; i < value.size(); ++i)
-        {
-            setpoint(i) = value.at(i);
-        }
-
-        LockGuardType guard {interfaceDataMutex};
-        interface2rtBuffer.getWriteBuffer().Dnull = setpoint;
-        interface2rtBuffer.commitWrite();
-
-    }
-
-    Ice::FloatSeq NJointTaskSpaceAdaptiveDMPController::getForce(const Ice::Current&)
-    {
-        Eigen::Vector3f force = interfaceData.getUpToDateReadBuffer().currentForce;
-        std::vector<float> forceVec = {force(0), force(1), force(2)};
-        return forceVec;
-    }
-
-    Ice::FloatSeq NJointTaskSpaceAdaptiveDMPController::getVelocityInMM(const Ice::Current&)
-    {
-        Eigen::VectorXf currentVel = interfaceData.getUpToDateReadBuffer().currentVel;
-        std::vector<float> tvelvec = {currentVel(0), currentVel(1), currentVel(2), currentVel(3), currentVel(4), currentVel(5)};
-        return tvelvec;
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::stopDMP(const Ice::Current&)
-    {
-        oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
-        stopped = true;
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::resumeDMP(const Ice::Current&)
-    {
-        stopped = false;
-    }
-
-
-    void NJointTaskSpaceAdaptiveDMPController::removeAllViaPoints(const Ice::Current&)
-    {
-        LockGuardType guard {controllerMutex};
-        dmpCtrl->removeAllViaPoints();
-    }
-
-
-    void NJointTaskSpaceAdaptiveDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
-    {
-        firstRun = true;
-        while (firstRun)
-        {
-            usleep(100);
-        }
-
-        Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
-        dmpCtrl->canVal = timeDuration;
-        dmpCtrl->config.motionTimeDuration = timeDuration;
-
-        finished = false;
-
-        if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
-        {
-            ARMARX_INFO << "Using Null Space Joint DMP";
-        }
-
-        started = true;
-        stopped = false;
-        //        controllerTask->start();
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
-    {
-        firstRun = true;
-        while (firstRun)
-        {
-            usleep(100);
-        }
-
-        Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
-        finished = false;
-
-        if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
-        {
-            ARMARX_INFO << "Using Null Space Joint DMP";
-        }
-
-        started = true;
-        stopped = false;
-        //        controllerTask->start();
-    }
-
-
-
-    void NJointTaskSpaceAdaptiveDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-    {
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
-        for (auto& pair : values)
-        {
-            datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
-        }
-
-        auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
-        for (auto& pair : values_null)
-        {
-            datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
-        }
-
-        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
-        datafields["targetPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
-        datafields["targetPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
-        datafields["targetPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
-        datafields["targetPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
-        datafields["targetPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
-        datafields["targetPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
-        datafields["targetPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
-
-        datafields["impedanceKp_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_x);
-        datafields["impedanceKp_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_y);
-        datafields["impedanceKp_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_z);
-        datafields["impedanceKp_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_rx);
-        datafields["impedanceKp_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_ry);
-        datafields["impedanceKp_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_rz);
-
-        datafields["currentPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
-        datafields["currentPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
-        datafields["currentPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
-        datafields["currentPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
-        datafields["currentPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
-        datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
-        datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
-
-        datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
-        datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
-        datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
-        datafields["forceDesired_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
-        datafields["forceDesired_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
-        datafields["forceDesired_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
-
-        datafields["forceInRoot_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_x);
-        datafields["forceInRoot_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_y);
-        datafields["forceInRoot_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_z);
-
-        datafields["vel_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_x);
-        datafields["vel_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_y);
-        datafields["vel_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_z);
-
-        datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-        std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
-        debugObs->setDebugChannel(channelName, datafields);
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        //        controllerTask = new PeriodicTask<NJointTaskSpaceAdaptiveDMPController>(this, &NJointTaskSpaceAdaptiveDMPController::controllerRun, 1);
-        runTask("NJointTaskSpaceAdaptiveDMPController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-    }
-
-    void NJointTaskSpaceAdaptiveDMPController::onDisconnectNJointController()
-    {
-        //        controllerTask->stop();
-    }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h
deleted file mode 100644
index 19708e4ad2d0256c9c818c2ab4d0e06462633f51..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h
+++ /dev/null
@@ -1,248 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-namespace armarx
-{
-    TYPEDEF_PTRS_HANDLE(NJointTaskSpaceAdaptiveDMPController);
-    TYPEDEF_PTRS_HANDLE(NJointTaskSpaceAdaptiveDMPControllerControlData);
-
-    class NJointTaskSpaceAdaptiveDMPControllerControlData
-    {
-    public:
-        Eigen::VectorXf targetVel;
-        Eigen::Matrix4f targetPose;
-        Eigen::VectorXf desiredNullSpaceJointValues;
-        double canVal;
-        double mpcFactor;
-    };
-
-
-
-    /**
-     * @brief The NJointTaskSpaceAdaptiveDMPController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointTaskSpaceAdaptiveDMPController :
-        public NJointControllerWithTripleBuffer<NJointTaskSpaceAdaptiveDMPControllerControlData>,
-        public NJointTaskSpaceAdaptiveDMPControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointTaskSpaceAdaptiveDMPControllerConfigPtr;
-        NJointTaskSpaceAdaptiveDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
-        // NJointTaskSpaceAdaptiveDMPControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
-        bool isFinished(const Ice::Current&)
-        {
-            return finished;
-        }
-
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-
-        void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&);
-        void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
-        void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-
-        Ice::Double getVirtualTime(const Ice::Current&)
-        {
-            return dmpCtrl->canVal;
-        }
-
-        void stopDMP(const Ice::Current&);
-        void resumeDMP(const Ice::Current&);
-        void resetDMP(const Ice::Current&);
-
-        void setKdImpedance(const Ice::FloatSeq& dampings, const Ice::Current&);
-        void setKpImpedance(const Ice::FloatSeq& stiffness, const Ice::Current&);
-
-        void setKdNull(const Ice::FloatSeq& dnull, const Ice::Current&);
-        void setKpNull(const Ice::FloatSeq& knull, const Ice::Current&);
-        Ice::FloatSeq getForce(const Ice::Current&);
-        Ice::FloatSeq getVelocityInMM(const Ice::Current&);
-        void setCanVal(double canVal, const Ice::Current&);
-
-        void removeAllViaPoints(const Ice::Current&);
-        void setUseNullSpaceJointDMP(bool useJointDMP, const Ice::Current&);
-        void setDefaultJointValues(const Ice::FloatSeq& desiredJointVals, const Ice::Current&);
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
-        void onInitNJointController();
-        void onDisconnectNJointController();
-        void controllerRun();
-
-    private:
-        struct DebugBufferData
-        {
-            double currentCanVal;
-            double mpcfactor;
-            float targetPose_x;
-            float targetPose_y;
-            float targetPose_z;
-            float targetPose_qw;
-            float targetPose_qx;
-            float targetPose_qy;
-            float targetPose_qz;
-
-            float currentPose_x;
-            float currentPose_y;
-            float currentPose_z;
-            float currentPose_qw;
-            float currentPose_qx;
-            float currentPose_qy;
-            float currentPose_qz;
-
-            StringFloatDictionary desired_torques;
-            StringFloatDictionary desired_nullspaceJoint;
-            float forceDesired_x;
-            float forceDesired_y;
-            float forceDesired_z;
-            float forceDesired_rx;
-            float forceDesired_ry;
-            float forceDesired_rz;
-
-            float impedanceKp_x;
-            float impedanceKp_y;
-            float impedanceKp_z;
-            float impedanceKp_rx;
-            float impedanceKp_ry;
-            float impedanceKp_rz;
-
-            float forceInRoot_x;
-            float forceInRoot_y;
-            float forceInRoot_z;
-            //            float torqueInRoot_x;
-            //            float torqueInRoot_y;
-            //            float torqueInRoot_z;
-
-            float vel_x;
-            float vel_y;
-            float vel_z;
-
-            float deltaT;
-
-        };
-
-        TripleBuffer<DebugBufferData> debugOutputData;
-
-        struct NJointTaskSpaceAdaptiveDMPControllerSensorData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-        };
-        TripleBuffer<NJointTaskSpaceAdaptiveDMPControllerSensorData> controllerSensorData;
-
-        struct NJointTaskSpaceAdaptiveDMPControllerInterfaceData
-        {
-            Eigen::Matrix4f currentTcpPose;
-            Eigen::VectorXf currentVel;
-            Eigen::Vector3f currentForce;
-
-        };
-
-        TripleBuffer<NJointTaskSpaceAdaptiveDMPControllerInterfaceData> interfaceData;
-
-
-        struct Inferface2rtData
-        {
-            Eigen::VectorXf KpImpedance;
-            Eigen::VectorXf KdImpedance;
-            Eigen::VectorXf Knull;
-            Eigen::VectorXf Dnull;
-        };
-        TripleBuffer<Inferface2rtData> interface2rtBuffer;
-
-        struct Interface2CtrlData
-        {
-            double canVal;
-        };
-        TripleBuffer<Interface2CtrlData> interface2CtrlBuffer;
-
-
-        DMP::Vec<DMP::DMPState> currentJointState;
-        DMP::UMIDMPPtr nullSpaceJointDMPPtr;
-
-        TaskSpaceDMPControllerPtr dmpCtrl;
-
-        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-        std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
-        // velocity ik controller parameters
-        // dmp parameters
-        double timeDuration;
-        bool finished;
-        VirtualRobot::RobotNodeSetPtr rns;
-
-        // phaseStop parameters
-        double phaseL;
-        double phaseK;
-        double phaseDist0;
-        double phaseDist1;
-        double posToOriRatio;
-
-
-        NJointTaskSpaceAdaptiveDMPControllerConfigPtr cfg;
-        VirtualRobot::DifferentialIKPtr ik;
-        VirtualRobot::RobotNodePtr tcp;
-
-        float torqueLimit;
-
-        Eigen::Vector3f kpos;
-        Eigen::Vector3f kori;
-        Eigen::Vector3f dpos;
-        Eigen::Vector3f dori;
-        Eigen::VectorXf knull;
-        Eigen::VectorXf dnull;
-        int numOfJoints;
-
-        bool useNullSpaceJointDMP;
-        bool isNullSpaceJointDMPLearned;
-
-
-        Eigen::VectorXf defaultNullSpaceJointValues;
-        std::vector<std::string> jointNames;
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointTaskSpaceAdaptiveDMPController>::pointer_type controllerTask;
-        bool firstRun;
-        bool started = false;
-        bool stopped = false;
-        Eigen::Vector3f forceOffset;
-
-        Eigen::Matrix4f oldPose;
-        const SensorValueForceTorque* forceSensor;
-        Eigen::Vector3f filteredForce;
-
-        mutable MutexType interfaceDataMutex;
-        mutable MutexType int2ctrlMutex;
-
-        // NJointController interface
-    protected:
-        void rtPreActivateController();
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
deleted file mode 100644
index df6f5374d391a6bdf0ec9378df8668017599b4eb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ /dev/null
@@ -1,780 +0,0 @@
-#include "NJointTaskSpaceImpedanceDMPController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-
-namespace armarx
-{
-    NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController(
-        "NJointTaskSpaceImpedanceDMPController");
-
-    NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit,
-            const armarx::NJointControllerConfigPtr& config,
-            const VirtualRobot::RobotPtr&)
-    {
-        ARMARX_TRACE;
-        ARMARX_INFO << "creating impedance dmp controller";
-        cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
-        ARMARX_CHECK_NOT_NULL(cfg);
-        useSynchronizedRtRobot();
-        rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-        ARMARX_INFO << "1";
-        for (size_t i = 0; i < rns->getSize(); ++i)
-        {
-            std::string jointName = rns->getNode(i)->getName();
-            jointNames.push_back(jointName);
-            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
-            const SensorValueBase* sv = useSensorValue(jointName);
-            targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
-            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
-            if (!velocitySensor)
-            {
-                ARMARX_WARNING << "No velocitySensor available for " << jointName;
-            }
-            if (!positionSensor)
-            {
-                ARMARX_WARNING << "No positionSensor available for " << jointName;
-            }
-
-            velocitySensors.push_back(velocitySensor);
-            positionSensors.push_back(positionSensor);
-        };
-        const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
-        forceSensor = svlf->asA<SensorValueForceTorque>();
-
-        ARMARX_TRACE;
-        forceOffset.setZero();
-        filteredForce.setZero();
-        filteredForceInRoot.setZero();
-        ARMARX_INFO << cfg->forceThreshold;
-        forceThreshold.reinitAllBuffers(cfg->forceThreshold);
-        tcp =  rns->getTCP();
-        ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-        ik->setDampedSvdLambda(0.0001);
-
-        ARMARX_TRACE;
-        numOfJoints = targets.size();
-        // set DMP
-        TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
-        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
-        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
-        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
-        taskSpaceDMPConfig.DMPAmplitude = 1.0;
-        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
-        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
-        taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
-        taskSpaceDMPConfig.phaseStopParas.Kori = 0;
-        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
-        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
-        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-        dmpCtrl.reset(new TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
-        finished = false;
-
-        useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
-        nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
-
-        isNullSpaceJointDMPLearned = false;
-
-        Eigen::VectorXf nullspaceValues(targets.size());
-
-        ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            nullspaceValues(i) = cfg->defaultNullSpaceJointValues.at(i);
-        }
-        defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
-
-        ARMARX_TRACE;
-        Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
-        Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
-        Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
-        Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
-        Eigen::VectorXf knull(targets.size());
-        Eigen::VectorXf dnull(targets.size());
-
-        ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
-        ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
-
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            knull(i) = cfg->Knull.at(i);
-            dnull(i) = cfg->Dnull.at(i);
-        }
-
-        CtrlParams initParams = {kpos, dpos, kori, dori, knull, dnull};
-        ctrlParams.reinitAllBuffers(initParams);
-
-        torqueLimit = cfg->torqueLimit;
-        timeDuration = cfg->timeDuration;
-
-        NJointTaskSpaceImpedanceDMPControllerInterfaceData initInterfaceData;
-        initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
-        interfaceData.reinitAllBuffers(initInterfaceData);
-
-        NJointTaskSpaceImpedanceDMPControllerSensorData initControllerSensorData;
-        initControllerSensorData.currentPose = Eigen::Matrix4f::Identity();
-        initControllerSensorData.currentTime = 0;
-        initControllerSensorData.deltaT = 0;
-        initControllerSensorData.currentTwist.setZero();
-        controllerSensorData.reinitAllBuffers(initControllerSensorData);
-
-        firstRun = true;
-        useForceStop = false;
-
-        ARMARX_INFO << "Finished controller constructor ";
-    }
-
-    std::string NJointTaskSpaceImpedanceDMPController::getClassName(const Ice::Current&) const
-    {
-        return "NJointTaskSpaceImpedanceDMPController";
-
-    }
-
-
-    void NJointTaskSpaceImpedanceDMPController::rtPreActivateController()
-    {
-        ARMARX_TRACE;
-        NJointTaskSpaceImpedanceDMPControllerControlData initData;
-        initData.targetPose = tcp->getPoseInRootFrame();
-        initData.targetVel.resize(6);
-        initData.targetVel.setZero();
-        initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
-        reinitTripleBuffer(initData);
-
-
-    }
-
-
-    void NJointTaskSpaceImpedanceDMPController::controllerRun()
-    {
-        if (!dmpCtrl)
-        {
-            return;
-        }
-
-        if (!controllerSensorData.updateReadBuffer())
-        {
-            return;
-        }
-
-
-        double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
-        Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
-        Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
-
-        if (!started)
-        {
-            LockGuardType guard{controlDataMutex};
-            getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
-            getWriterControlStruct().targetVel.setZero(6);
-            getWriterControlStruct().targetPose = currentPose;
-            getWriterControlStruct().canVal = 1.0;
-            getWriterControlStruct().mpcFactor = 0.0;
-            writeControlStruct();
-        }
-        else
-        {
-            if (stopped)
-            {
-
-                LockGuardType guard{controlDataMutex};
-                getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
-                getWriterControlStruct().targetVel.setZero(6);
-                getWriterControlStruct().targetPose = oldPose;
-                getWriterControlStruct().canVal = dmpCtrl->canVal;
-                getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
-                writeControlStruct();
-            }
-            else
-            {
-                if (dmpCtrl->canVal < 1e-8)
-                {
-                    finished = true;
-                    LockGuardType guard{controlDataMutex};
-                    getWriterControlStruct().targetVel.setZero();
-                    writeControlStruct();
-                    return;
-                }
-
-                dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
-                Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
-                if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
-                {
-                    DMP::DVec targetJointState;
-                    currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState,
-                                        dmpCtrl->canVal / timeDuration,
-                                        deltaT / timeDuration,
-                                        targetJointState);
-
-                    if (targetJointState.size() == jointNames.size())
-                    {
-                        for (size_t i = 0; i < targetJointState.size(); ++i)
-                        {
-                            desiredNullSpaceJointValues(i) = targetJointState[i];
-                        }
-                    }
-                    else
-                    {
-                        desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
-                    }
-                }
-                else
-                {
-                    desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
-                }
-
-                LockGuardType guard{controlDataMutex};
-                getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
-                getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
-                getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
-                getWriterControlStruct().canVal = dmpCtrl->canVal;
-                getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
-
-                writeControlStruct();
-            }
-        }
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
-            const IceUtil::Time& timeSinceLastIteration)
-    {
-
-        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
-
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
-        Eigen::Matrix4f targetPose;
-        Eigen::VectorXf targetVel;
-        Eigen::VectorXf desiredNullSpaceJointValues;
-        if (firstRun)
-        {
-            firstRun = false;
-            targetPose = currentPose;
-            stopPose = currentPose;
-            targetVel.setZero(6);
-            desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
-        }
-        else
-        {
-            if (!started)
-            {
-                targetPose = stopPose;
-                targetVel.setZero(6);
-                desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
-                forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
-                timeForCalibration = timeForCalibration + timeSinceLastIteration.toSecondsDouble();
-            }
-            else
-            {
-                targetPose = rtGetControlStruct().targetPose;
-                targetVel = rtGetControlStruct().targetVel;
-                desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
-
-                if (useForceStop)
-                {
-                    /* handle force stop */
-                    filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
-
-                    for (size_t i = 0; i < 3; ++i)
-                    {
-                        if (fabs(filteredForce(i)) > cfg->forceDeadZone)
-                        {
-                            filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
-                        }
-                        else
-                        {
-                            filteredForce(i) = 0;
-                        }
-                    }
-                    Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
-                    filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
-
-                    for (size_t i = 0; i < 3; ++i)
-                    {
-                        if (fabs(filteredForceInRoot[i]) > forceThreshold.getUpToDateReadBuffer()[i])
-                        {
-                            stopPose = currentPose;
-                            targetVel.setZero(6);
-                            desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
-                            started = false;
-                            break;
-                        }
-                    }
-                }
-
-            }
-        }
-
-
-
-        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-        Eigen::VectorXf qpos(positionSensors.size());
-        Eigen::VectorXf qvel(velocitySensors.size());
-        for (size_t i = 0; i < positionSensors.size(); ++i)
-        {
-            qpos(i) = positionSensors[i]->position;
-            qvel(i) = velocitySensors[i]->velocity;
-        }
-
-        Eigen::VectorXf currentTwist = jacobi * qvel;
-
-        controllerSensorData.getWriteBuffer().currentPose = currentPose;
-        controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
-        controllerSensorData.getWriteBuffer().deltaT = deltaT;
-        controllerSensorData.getWriteBuffer().currentTime += deltaT;
-        controllerSensorData.commitWrite();
-
-        interfaceData.getWriteBuffer().currentTcpPose = currentPose;
-        interfaceData.commitWrite();
-
-        jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
-
-        Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
-        Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
-        Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
-        Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
-        Eigen::VectorXf knull = ctrlParams.getUpToDateReadBuffer().knull;
-        Eigen::VectorXf dnull = ctrlParams.getUpToDateReadBuffer().dnull;
-
-        Eigen::Vector6f jointControlWrench;
-        {
-            Eigen::Vector3f targetTCPLinearVelocity;
-            targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
-            Eigen::Vector3f currentTCPLinearVelocity;
-            currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
-            Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
-            Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
-            Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
-                                              dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-
-            Eigen::Vector3f currentTCPAngularVelocity;
-            currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
-            Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
-            Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
-            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-            Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
-            jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
-        }
-
-        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
-
-        Eigen::VectorXf nullspaceTorque =
-            knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
-        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
-        Eigen::VectorXf jointDesiredTorques =
-            jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
-
-        // torque limit
-        ARMARX_CHECK_EXPRESSION(!targets.empty());
-        ARMARX_CHECK_LESS(targets.size(), 1000);
-        for (size_t i = 0; i < targets.size(); ++i)
-        {
-            float desiredTorque = jointDesiredTorques(i);
-
-            if (isnan(desiredTorque))
-            {
-                desiredTorque = 0;
-            }
-
-            desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
-            desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
-            debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
-            debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] = desiredNullSpaceJointValues(i);
-
-            targets.at(i)->torque = desiredTorque;
-            if (!targets.at(i)->isValid())
-            {
-                ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: "
-                            << targets.at(i)->torque;
-                targets.at(i)->torque = 0.0f;
-            }
-        }
-
-
-        debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
-        debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
-        debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
-        debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
-        debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
-        debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
-
-        //        debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
-        //        debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
-
-        debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
-        debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
-        debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
-        VirtualRobot::MathTools::Quaternion targetQuat = VirtualRobot::MathTools::eigen4f2quat(targetPose);
-        debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
-        debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
-        debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
-        debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
-        debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
-
-        debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
-        debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
-        debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
-        VirtualRobot::MathTools::Quaternion currentQuat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
-        debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
-        debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
-        debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
-        debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
-        debugOutputData.getWriteBuffer().deltaT = deltaT;
-
-        debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
-        debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
-        debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
-        debugOutputData.getWriteBuffer().currentKori_x = kori.x();
-        debugOutputData.getWriteBuffer().currentKori_y = kori.y();
-        debugOutputData.getWriteBuffer().currentKori_z = kori.z();
-        debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
-        debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
-        debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
-
-        debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
-        debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
-        debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
-        debugOutputData.getWriteBuffer().currentDori_x = dori.x();
-        debugOutputData.getWriteBuffer().currentDori_y = dori.y();
-        debugOutputData.getWriteBuffer().currentDori_z = dori.z();
-        debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
-        debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
-        debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
-
-        debugOutputData.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
-
-        debugOutputData.commitWrite();
-
-    }
-
-
-    void NJointTaskSpaceImpedanceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
-    {
-        dmpCtrl->learnDMPFromFiles(fileNames);
-        ARMARX_INFO << "Learned DMP ... ";
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint,
-            const Ice::Current&)
-    {
-        LockGuardType guard(controllerMutex);
-        ARMARX_INFO << "setting via points ";
-        dmpCtrl->setViaPose(u, viapoint);
-
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
-    {
-        dmpCtrl->setGoalPoseVec(goals);
-
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName,
-            const Ice::FloatSeq& currentJVS,
-            const Ice::Current&)
-    {
-        DMP::Vec<DMP::SampledTrajectoryV2> trajs;
-        DMP::DVec ratios;
-        DMP::SampledTrajectoryV2 traj;
-        traj.readFromCSVFile(fileName);
-        traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
-        if (traj.dim() != jointNames.size())
-        {
-            isNullSpaceJointDMPLearned = false;
-            return;
-        }
-
-        DMP::DVec goal;
-        goal.resize(traj.dim());
-        currentJointState.resize(traj.dim());
-
-        for (size_t i = 0; i < goal.size(); ++i)
-        {
-            goal.at(i) = traj.rbegin()->getPosition(i);
-            currentJointState.at(i).pos = currentJVS.at(i);
-            currentJointState.at(i).vel = 0;
-        }
-
-        trajs.push_back(traj);
-        nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
-
-        // prepare exeuction of joint dmp
-        nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
-        ARMARX_INFO << "prepared nullspace joint dmp";
-        isNullSpaceJointDMPLearned = true;
-    }
-
-
-    void NJointTaskSpaceImpedanceDMPController::resetDMP(const Ice::Current&)
-    {
-        if (started)
-        {
-            ARMARX_INFO << "Cannot reset running DMP";
-        }
-        firstRun = true;
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::stopDMP(const Ice::Current&)
-    {
-        oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
-        stopped = true;
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::resumeDMP(const Ice::Current&)
-    {
-        stopped = false;
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP(bool enable, const Ice::Current&)
-    {
-        useNullSpaceJointDMP = enable;
-    }
-
-
-    void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration,
-            const Ice::Current&)
-    {
-        dmpCtrl->canVal = timeDuration;
-        dmpCtrl->config.motionTimeDuration = timeDuration;
-
-        runDMP(goals);
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
-    {
-        firstRun = true;
-        timeForCalibration = 0;
-        started = false;
-
-        while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
-        {
-            usleep(100);
-        }
-
-        while (!interfaceData.updateReadBuffer())
-        {
-            usleep(100);
-        }
-
-        Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
-        finished = false;
-
-        if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
-        {
-            ARMARX_INFO << "Using Null Space Joint DMP";
-        }
-
-        started = true;
-        stopped = false;
-        //        controllerTask->start();
-    }
-
-
-    void NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&,
-            const DebugObserverInterfacePrx& debugObs)
-    {
-        StringVariantBaseMap datafields;
-        auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
-        for (auto& pair : values)
-        {
-            datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
-        }
-
-        auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
-        for (auto& pair : values_null)
-        {
-            datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
-        }
-
-        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-        datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
-        datafields["targetPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
-        datafields["targetPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
-        datafields["targetPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
-        datafields["targetPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
-        datafields["targetPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
-        datafields["targetPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
-        datafields["targetPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
-
-        datafields["currentPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
-        datafields["currentPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
-        datafields["currentPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
-        datafields["currentPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
-        datafields["currentPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
-        datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
-        datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
-
-        datafields["currentKpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
-        datafields["currentKpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
-        datafields["currentKpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
-        datafields["currentKori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
-        datafields["currentKori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
-        datafields["currentKori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
-        datafields["currentKnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
-        datafields["currentKnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
-        datafields["currentKnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
-
-        datafields["currentDpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
-        datafields["currentDpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
-        datafields["currentDpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
-        datafields["currentDori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
-        datafields["currentDori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
-        datafields["currentDori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
-        datafields["currentDnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
-        datafields["currentDnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
-        datafields["currentDnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
-
-        datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
-        datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
-        datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
-        datafields["forceDesired_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
-        datafields["forceDesired_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
-        datafields["forceDesired_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
-
-        datafields["filteredForceInRoot_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[0]);
-        datafields["filteredForceInRoot_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[1]);
-        datafields["filteredForceInRoot_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[2]);
-
-        datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-        std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
-        debugObs->setDebugChannel(channelName, datafields);
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::onInitNJointController()
-    {
-        ARMARX_INFO << "init ...";
-        //        controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 1);
-        runTask("NJointTaskSpaceImpedanceDMPController", [&]
-        {
-            CycleUtil c(1);
-            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-            while (getState() == eManagedIceObjectStarted)
-            {
-                if (isControllerActive())
-                {
-                    controllerRun();
-                }
-                c.waitForCycleDuration();
-            }
-        });
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::onDisconnectNJointController()
-    {
-        //        controllerTask->stop();
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
-    {
-        dmpCtrl->setWeights(weights);
-    }
-
-    DoubleSeqSeq NJointTaskSpaceImpedanceDMPController::getMPWeights(const Ice::Current&)
-    {
-        DMP::DVec2d res = dmpCtrl->getWeights();
-        DoubleSeqSeq resvec;
-        for (size_t i = 0; i < res.size(); ++i)
-        {
-            std::vector<double> cvec;
-            for (size_t j = 0; j < res[i].size(); ++j)
-            {
-                cvec.push_back(res[i][j]);
-            }
-            resvec.push_back(cvec);
-        }
-
-        return resvec;
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
-    {
-        LockGuardType guard{controllerMutex};
-        ARMARX_INFO << "setting via points ";
-        dmpCtrl->removeAllViaPoints();
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
-    {
-        ARMARX_CHECK_EQUAL(kd.size(), 3);
-        ARMARX_INFO << "set linear kd " << VAROUT(kd);
-        LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().dpos = kd;
-        ctrlParams.commitWrite();
-
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
-    {
-        ARMARX_CHECK_EQUAL(kp.size(), 3);
-        ARMARX_INFO << "set linear kp " << VAROUT(kp);
-        LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().kpos = kp;
-        ctrlParams.commitWrite();
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
-    {
-        ARMARX_CHECK_EQUAL(kd.size(), 3);
-        ARMARX_INFO << "set angular kd " << VAROUT(kd);
-        LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().dori = kd;
-        ctrlParams.commitWrite();
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
-    {
-        ARMARX_CHECK_EQUAL(kp.size(), 3);
-        ARMARX_INFO << "set angular kp " << VAROUT(kp);
-        LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().kori = kp;
-        ctrlParams.commitWrite();
-
-
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&)
-    {
-        ARMARX_CHECK_EQUAL((std::size_t)kd.size(), targets.size());
-        ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
-        LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().dnull = kd;
-        ctrlParams.commitWrite();
-    }
-
-    void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&)
-    {
-        ARMARX_CHECK_EQUAL((std::size_t)kp.size(), targets.size());
-        ARMARX_INFO << "set linear kp " << VAROUT(kp);
-        LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().knull = kp;
-        ctrlParams.commitWrite();
-    }
-
-
-    void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
-            const Ice::Current&)
-    {
-        ARMARX_CHECK_EQUAL((std::size_t)jointValues.size(), targets.size());
-        defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
-        defaultNullSpaceJointValues.commitWrite();
-
-    }
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
deleted file mode 100644
index 5361dec0f37c7269c86432c6dff64c95504cca3f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ /dev/null
@@ -1,272 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-namespace armarx
-{
-    TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPController);
-    TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPControllerControlData);
-
-    class NJointTaskSpaceImpedanceDMPControllerControlData
-    {
-    public:
-        Eigen::VectorXf targetVel;
-        Eigen::Matrix4f targetPose;
-        Eigen::VectorXf desiredNullSpaceJointValues;
-        double canVal;
-        double mpcFactor;
-    };
-
-
-
-    /**
-     * @brief The NJointTaskSpaceImpedanceDMPController class
-     * @ingroup Library-RobotUnit-NJointControllers
-     */
-    class NJointTaskSpaceImpedanceDMPController :
-        public NJointControllerWithTripleBuffer<NJointTaskSpaceImpedanceDMPControllerControlData>,
-        public NJointTaskSpaceImpedanceDMPControllerInterface
-    {
-    public:
-        using ConfigPtrT = NJointTaskSpaceImpedanceDMPControllerConfigPtr;
-        NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-        // NJointControllerInterface interface
-        std::string getClassName(const Ice::Current&) const override;
-
-        // NJointController interface
-
-        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
-        // NJointTaskSpaceImpedanceDMPControllerInterface interface
-        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
-        bool isFinished(const Ice::Current&) override
-        {
-            return finished;
-        }
-
-        bool isDMPRunning(const Ice::Current&) override
-        {
-            return started;
-        }
-
-        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
-        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
-        void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&) override;
-        void runDMP(const Ice::DoubleSeq& goals, const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
-        void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
-
-        Ice::Double getVirtualTime(const Ice::Current&) override
-        {
-            return dmpCtrl->canVal;
-        }
-
-        void stopDMP(const Ice::Current&) override;
-        void resumeDMP(const Ice::Current&) override;
-        void resetDMP(const Ice::Current&) override;
-
-        void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
-        DoubleSeqSeq getMPWeights(const Ice::Current&) override;
-
-        void removeAllViaPoints(const Ice::Current&) override;
-
-        void setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
-        void setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
-        void setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
-        void setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
-        void setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&) override;
-        void setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&) override;
-        void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
-        void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
-
-        void enableForceStop(const Ice::Current&) override
-        {
-            useForceStop = true;
-        }
-        void disableForceStop(const Ice::Current&) override
-        {
-            useForceStop = false;
-        }
-
-        void setForceThreshold(const Eigen::Vector3f& f, const Ice::Current& current) override
-        {
-            forceThreshold.getWriteBuffer() = f;
-            forceThreshold.commitWrite();
-        }
-    protected:
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
-        void onInitNJointController() override;
-        void onDisconnectNJointController() override;
-        void controllerRun();
-
-    private:
-        struct DebugBufferData
-        {
-            double currentCanVal;
-            double mpcfactor;
-            float targetPose_x;
-            float targetPose_y;
-            float targetPose_z;
-            float targetPose_qw;
-            float targetPose_qx;
-            float targetPose_qy;
-            float targetPose_qz;
-
-            float currentPose_x;
-            float currentPose_y;
-            float currentPose_z;
-            float currentPose_qw;
-            float currentPose_qx;
-            float currentPose_qy;
-            float currentPose_qz;
-
-            float currentKpos_x;
-            float currentKpos_y;
-            float currentKpos_z;
-            float currentKori_x;
-            float currentKori_y;
-            float currentKori_z;
-            float currentKnull_x;
-            float currentKnull_y;
-            float currentKnull_z;
-
-            float currentDpos_x;
-            float currentDpos_y;
-            float currentDpos_z;
-            float currentDori_x;
-            float currentDori_y;
-            float currentDori_z;
-            float currentDnull_x;
-            float currentDnull_y;
-            float currentDnull_z;
-
-            StringFloatDictionary desired_torques;
-            StringFloatDictionary desired_nullspaceJoint;
-            float forceDesired_x;
-            float forceDesired_y;
-            float forceDesired_z;
-            float forceDesired_rx;
-            float forceDesired_ry;
-            float forceDesired_rz;
-
-            Eigen::Vector3f filteredForceInRoot;
-
-            float deltaT;
-
-
-
-        };
-
-        WriteBufferedTripleBuffer<DebugBufferData> debugOutputData;
-
-        struct NJointTaskSpaceImpedanceDMPControllerSensorData
-        {
-            double currentTime;
-            double deltaT;
-            Eigen::Matrix4f currentPose;
-            Eigen::VectorXf currentTwist;
-        };
-        WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
-
-        struct NJointTaskSpaceImpedanceDMPControllerInterfaceData
-        {
-            Eigen::Matrix4f currentTcpPose;
-        };
-
-        WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
-
-        struct CtrlParams
-        {
-            Eigen::Vector3f kpos;
-            Eigen::Vector3f dpos;
-            Eigen::Vector3f kori;
-            Eigen::Vector3f dori;
-            Eigen::VectorXf knull;
-            Eigen::VectorXf dnull;
-        };
-
-        WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
-
-
-
-        DMP::Vec<DMP::DMPState> currentJointState;
-        DMP::UMIDMPPtr nullSpaceJointDMPPtr;
-
-        TaskSpaceDMPControllerPtr dmpCtrl;
-
-        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
-        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
-        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-        std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
-        // velocity ik controller parameters
-        // dmp parameters
-        double timeDuration;
-        bool finished;
-        VirtualRobot::RobotNodeSetPtr rns;
-
-        // phaseStop parameters
-        double phaseL;
-        double phaseK;
-        double phaseDist0;
-        double phaseDist1;
-        double posToOriRatio;
-
-
-        NJointTaskSpaceImpedanceDMPControllerConfigPtr cfg;
-        VirtualRobot::DifferentialIKPtr ik;
-        VirtualRobot::RobotNodePtr tcp;
-
-        float torqueLimit;
-
-        //        Eigen::Vector3f kpos;
-        //        Eigen::Vector3f kori;
-        //        Eigen::Vector3f dpos;
-        //        Eigen::Vector3f dori;
-        //        Eigen::VectorXf knull;
-        //        Eigen::VectorXf dnull;
-        int numOfJoints;
-
-        std::atomic_bool useNullSpaceJointDMP;
-        bool isNullSpaceJointDMPLearned;
-
-
-        WriteBufferedTripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
-        std::vector<std::string> jointNames;
-        mutable MutexType controllerMutex;
-        PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
-        bool firstRun;
-        bool started = false;
-        bool stopped = false;
-
-        Eigen::Matrix4f stopPose;
-
-        Eigen::Vector3f filteredForce;
-        Eigen::Vector3f forceOffset;
-        Eigen::Vector3f filteredForceInRoot;
-        WriteBufferedTripleBuffer<Eigen::Vector3f> forceThreshold;
-        std::atomic<bool> useForceStop;
-        std::atomic<float> timeForCalibration;
-        const SensorValueForceTorque* forceSensor;
-
-        Eigen::Matrix4f oldPose;
-        // NJointController interface
-    protected:
-        void rtPreActivateController();
-    };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt
deleted file mode 100644
index 3afbfc395b3842db49d0e2a4d3741104c8528b81..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore RobotAPINJointControllers)
- 
-armarx_add_test(RobotAPINJointsControllerTest RobotAPINJointsControllerTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/test/RobotAPINJointsControllerTest.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/test/RobotAPINJointsControllerTest.cpp
deleted file mode 100644
index 2e8c1a22b837faaacf898daf2a63912d4d75daf1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/test/RobotAPINJointsControllerTest.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI::ArmarXObjects::RobotAPINJointsController
- * @author     zhou ( you dot zhou at kit dot edu )
- * @date       2018
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::RobotAPINJointsController
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
-    BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index 30c58aeca6c5f0374705a481be22db6e28645792..9b34e11bd3d53e896f38cd2c3301c2c35dcdffd9 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -5,6 +5,9 @@
 
 #include <SimoxUtility/algorithm/get_map_keys_values.h>
 
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
 #include <RobotAPI/libraries/armem/core/MemoryID.h>
 #include <RobotAPI/libraries/armem/core/Time.h>
 
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index 426511a9a8a59dd5cba9545f08f8479c0104d96c..1bf28496f697da38e2172a41836ecaccceaa5561 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -109,18 +109,5 @@ namespace armarx::armem
         dto.objectJointValues = bo.jointMap;
     }
     
-    void robot::fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo) 
-    {
-        bo.twist.linear.setZero();
-        bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
-
-        bo.twist.angular.setZero();
-        bo.twist.angular.z() = dto.velocity.z(); // yaw
-    }
-    
-    void robot::toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo) 
-    {
-        ARMARX_ERROR << "Not implemented yet.";
-    }
 
 }  // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index e6b462fa866d0c011cb0d899f08ad4e804e3f210..f2ea2ba959897ad48d9aed390a569d2e8009dd17 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -8,7 +8,6 @@
 #include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
 
 
 namespace armarx::armem::robot
@@ -27,9 +26,6 @@ namespace armarx::armem::robot
     void fromAron(const arondto::RobotState& dto, RobotState& bo);
     void toAron(arondto::RobotState& dto, const RobotState& bo);
 
-    void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
-    void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
-
     void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
     void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
 
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index ad7454d6fb4f74b49b6909a3a6d362c302203fc9..0a8473073f5b30dce3cb9f2d96682b4184a6a9b3 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -35,6 +35,12 @@ namespace armarx::armem::robot
         Twist twist;
     };
 
+    struct ForceTorque
+    {
+        Eigen::Vector3f force;
+        Eigen::Vector3f torque;
+    };
+
     struct RobotState
     {
         using JointMap = std::map<std::string, float>;
@@ -46,6 +52,7 @@ namespace armarx::armem::robot
         JointMap jointMap;
     };
 
+
     struct Robot
     {
         RobotDescription description;
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 785d25bdca4757ebe1c07f0bab767aa08a02a24d..03a578fab3b9250f724efcd646c24400d8a898d2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -1,31 +1,30 @@
 #include "aron_conversions.h"
 
-// STL
 #include <string>
 
-// Ice
 #include <IceUtil/Time.h>
 
-// RobotAPI
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <ArmarXCore/core/logging/Logging.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
-#include "RobotAPI/libraries/armem_robot_state/types.h"
-
 namespace armarx::armem
 {
 
     /* Transform */
 
-    void fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+    void
+    fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
     {
         fromAron(dto.header, bo.header);
         aron::fromAron(dto.transform, bo.transform);
     }
 
-    void toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+    void
+    toAron(arondto::Transform& dto, const robot_state::Transform& bo)
     {
         toAron(dto.header, bo.header);
         aron::toAron(dto.transform, bo.transform);
@@ -33,7 +32,8 @@ namespace armarx::armem
 
     /* TransformHeader */
 
-    void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+    void
+    toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
     {
         aron::toAron(dto.parentFrame, bo.parentFrame);
         aron::toAron(dto.frame, bo.frame);
@@ -41,7 +41,8 @@ namespace armarx::armem
         dto.timestamp = bo.timestamp;
     }
 
-    void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+    void
+    fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
     {
         aron::fromAron(dto.parentFrame, bo.parentFrame);
         aron::fromAron(dto.frame, bo.frame);
@@ -51,16 +52,49 @@ namespace armarx::armem
 
     /* JointState */
 
-    void fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
+    void
+    fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
     {
         aron::fromAron(dto.name, bo.name);
         aron::fromAron(dto.position, bo.position);
     }
 
-    void toAron(arondto::JointState& dto, const robot_state::JointState& bo)
+    void
+    toAron(arondto::JointState& dto, const robot_state::JointState& bo)
     {
         aron::toAron(dto.name, bo.name);
         aron::toAron(dto.position, bo.position);
     }
 
-}  // namespace armarx::armem
+
+    void
+    fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+    {
+        bo.twist.linear.setZero();
+        bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
+
+        bo.twist.angular.setZero();
+        bo.twist.angular.z() = dto.velocity.z(); // yaw
+    }
+
+    void
+    toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+    {
+        ARMARX_ERROR << "Not implemented yet.";
+    }
+
+    void
+    fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
+    {
+        bo.force = dto.force;
+        bo.torque = dto.torque;
+    }
+
+    void
+    toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
+    {
+        dto.force = bo.force;
+        dto.torque = bo.torque;
+    }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 202a81f089702e07155232200911fa0e0afa197d..17fe89152ef063ab932ace41a9ade84eb2f2f556 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -21,6 +21,9 @@
 
 #pragma once
 
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
 namespace armarx::armem
 {
     namespace robot_state
@@ -41,6 +44,7 @@ namespace armarx::armem
 
     } // namespace arondto
 
+
     void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
     void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
 
@@ -50,4 +54,11 @@ namespace armarx::armem
     void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
     void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
 
-}  // namespace armarx::armem
\ No newline at end of file
+    void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
+    void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
+
+    void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
+    void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+
+
+}  // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 4da91df52e77ecd83fe4c061d0a969bce8bae32f..2cd6bad5240f6e5a4dbbb436711fcf6bfb439abc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -5,33 +5,34 @@
 
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 #include "RobotAPI/libraries/armem_robot/types.h"
+#include <ArmarXCore/core/PackagePath.h>
 #include <ArmarXCore/core/exceptions/LocalException.h>
 #include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/PackagePath.h>
-
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/core/error.h>
 #include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
 #include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot/aron_conversions.h>
 #include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
 
 
 namespace fs = ::std::filesystem;
 
 namespace armarx::armem::robot_state
 {
-   
+
 
     RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
         memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
     {
     }
 
-    void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+    void
+    RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
     {
         transformReader.registerPropertyDefinitions(def);
 
@@ -42,7 +43,8 @@ namespace armarx::armem::robot_state
                       propertyPrefix + "proprioceptionSegment");
     }
 
-    void RobotReader::connect()
+    void
+    RobotReader::connect()
     {
         transformReader.connect();
 
@@ -51,7 +53,8 @@ namespace armarx::armem::robot_state
         try
         {
             memoryReader = memoryNameSystem.useReader(properties.memoryName);
-            ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName << "'";
+            ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName
+                             << "'";
         }
         catch (const armem::error::CouldNotResolveMemoryServer& e)
         {
@@ -60,8 +63,8 @@ namespace armarx::armem::robot_state
         }
     }
 
-    std::optional<robot::Robot> RobotReader::get(const std::string& name,
-            const armem::Time& timestamp)
+    std::optional<robot::Robot>
+    RobotReader::get(const std::string& name, const armem::Time& timestamp)
     {
         const auto description = queryDescription(name, timestamp);
 
@@ -74,20 +77,21 @@ namespace armarx::armem::robot_state
         return get(*description, timestamp);
     }
 
-    robot::Robot RobotReader::get(const robot::RobotDescription& description,
-                                  const armem::Time& timestamp)
+    robot::Robot
+    RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp)
     {
         robot::Robot robot{.description = description,
-                           .instance    = "", // TODO(fabian.reister):
-                           .config      = {}, // will be populated by synchronize
-                           .timestamp   = timestamp};
+                           .instance = "", // TODO(fabian.reister):
+                           .config = {}, // will be populated by synchronize
+                           .timestamp = timestamp};
 
         synchronize(robot, timestamp);
 
         return robot;
     }
 
-    bool RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
+    bool
+    RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
     {
         auto state = queryState(obj.description, timestamp);
 
@@ -119,7 +123,8 @@ namespace armarx::armem::robot_state
 
         if (not memoryReader)
         {
-            ARMARX_WARNING << "Memory reader is null. Did you forget to call RobotReader::connect() in onConnectComponent()?";
+            ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+                              "RobotReader::connect() in onConnectComponent()?";
             return std::nullopt;
         }
 
@@ -162,8 +167,7 @@ namespace armarx::armem::robot_state
             return std::nullopt;
         }
 
-        return robot::RobotState
-        {
+        return robot::RobotState{
             .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
     }
 
@@ -199,11 +203,41 @@ namespace armarx::armem::robot_state
         return getRobotJointState(qResult.memory, description.name);
     }
 
+    RobotReader::JointTrajectory
+    RobotReader::queryJointStates(const robot::RobotDescription& description,
+                                  const armem::Time& begin,
+                                  const armem::Time& end) const
+    {
+        armem::client::query::Builder qb;
+
+        ARMARX_DEBUG << "Querying robot joint states for robot: `" << description
+                     << "` on time interval [" << begin << "," << end << "]";
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.proprioceptionCoreSegment)
+        .providerSegments().withName(description.name) // agent
+        .entities().all() // TODO
+        .snapshots().timeRange(begin, end);
+        // clang-format on
+
+        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+        if (not qResult.success) /* c++20 [[unlikely]] */
+        {
+            ARMARX_WARNING << qResult.errorMessage;
+            return {};
+        }
+
+        return getRobotJointStates(qResult.memory, description.name);
+    }
 
 
     std::optional<robot::PlatformState>
     RobotReader::queryPlatformState(const robot::RobotDescription& description,
-                                 const armem::Time& timestamp) const
+                                    const armem::Time& timestamp) const
     {
         // TODO(fabian.reister): how to deal with multiple providers?
 
@@ -266,11 +300,13 @@ namespace armarx::armem::robot_state
         // clang-format on
 
         const armem::wm::EntityInstance* instance = nullptr;
-        providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
-        {
-            instance = &i;
-            return false;  // break
-        });
+        providerSegment.forEachInstance(
+            [&instance](const wm::EntityInstance& i)
+            {
+                instance = &i;
+                return false; // break
+            });
+
         if (!instance)
         {
             ARMARX_WARNING << "No entity snapshots found";
@@ -283,10 +319,10 @@ namespace armarx::armem::robot_state
 
     // FIXME remove this, use armem/util/util.h
     template <typename AronClass>
-    std::optional<AronClass> tryCast(const wm::EntityInstance& item)
+    std::optional<AronClass>
+    tryCast(const wm::EntityInstance& item)
     {
-        static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
-                      AronClass>::value);
+        static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass, AronClass>::value);
 
         try
         {
@@ -312,28 +348,29 @@ namespace armarx::armem::robot_state
                 .getCoreSegment(properties.proprioceptionCoreSegment);
         // clang-format on
 
-        coreSegment.forEachEntity([&jointMap](const wm::Entity & entity)
-        {
-            const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+        coreSegment.forEachEntity(
+            [&jointMap](const wm::Entity& entity)
+            {
+                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
-            const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
-            ARMARX_CHECK(proprioception.has_value());
+                const auto proprioception =
+                    tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+                ARMARX_CHECK(proprioception.has_value());
 
-            const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
-            
+                const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
 
 
-            // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
-            // if (not jointState)
-            // {
-            //     ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
-            //     return;
-            // }
+                // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+                // if (not jointState)
+                // {
+                //     ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+                //     return;
+                // }
 
-            jointMap = joints.position;
+                jointMap = joints.position;
 
-            // jointMap.emplace(jointState->name, jointState->position);
-        });
+                // jointMap.emplace(jointState->name, jointState->position);
+            });
 
         if (jointMap.empty())
         {
@@ -343,32 +380,223 @@ namespace armarx::armem::robot_state
         return jointMap;
     }
 
-     std::optional<robot::PlatformState>
-    RobotReader::getRobotPlatformState (const armarx::armem::wm::Memory& memory,
-                                    const std::string& name) const
+    RobotReader::JointTrajectory
+    RobotReader::getRobotJointStates(const armarx::armem::wm::Memory& memory,
+                                     const std::string& name) const
     {
-        std::optional<robot::PlatformState> platformState;
+
+        RobotReader::JointTrajectory jointTrajectory;
 
         // clang-format off
         const armem::wm::CoreSegment& coreSegment = memory
                 .getCoreSegment(properties.proprioceptionCoreSegment);
         // clang-format on
 
-        coreSegment.forEachEntity([&platformState](const wm::Entity & entity)
+        coreSegment.forEachEntity(
+            [&jointTrajectory](const wm::Entity& entity)
+            {
+                entity.forEachSnapshot(
+                    [&](const auto& snapshot)
+                    {
+                        if (not snapshot.hasInstance(0))
+                        {
+                            return;
+                        }
+
+                        const auto& entityInstance = snapshot.getInstance(0);
+
+                        const auto proprioception =
+                            tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+                        ARMARX_CHECK(proprioception.has_value());
+
+                        const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
+
+                        jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
+                    });
+            });
+
+        ARMARX_INFO << "Joint trajectory with " << jointTrajectory.size() << " elements";
+
+        return jointTrajectory;
+    }
+
+
+    // force torque for left and right
+    std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+    RobotReader::queryForceTorque(const robot::RobotDescription& description,
+                                  const armem::Time& timestamp) const
+    {
+
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.proprioceptionCoreSegment)
+        .providerSegments().withName(description.name) // agent
+        .entities().all() // TODO
+        .snapshots().beforeOrAtTime(timestamp);
+        // clang-format on
+
+        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+        if (not qResult.success) /* c++20 [[unlikely]] */
         {
-            const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+            ARMARX_WARNING << qResult.errorMessage;
+            return std::nullopt;
+        }
 
-            const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
-            ARMARX_CHECK(proprioception.has_value());
+        return getForceTorque(qResult.memory, description.name);
+    }
 
-            platformState = robot::PlatformState(); // initialize optional
-            robot::fromAron(proprioception->platform, platformState.value());
+    // force torque for left and right
+    std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
+    RobotReader::queryForceTorques(const robot::RobotDescription& description,
+                                   const armem::Time& start,
+                                   const armem::Time& end) const
+    {
 
-        });
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.proprioceptionCoreSegment)
+        .providerSegments().withName(description.name) // agent
+        .entities().all() // TODO
+        .snapshots().timeRange(start, end);
+        // clang-format on
+
+        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+        if (not qResult.success) /* c++20 [[unlikely]] */
+        {
+            ARMARX_WARNING << qResult.errorMessage;
+            return std::nullopt;
+        }
+
+        return getForceTorques(qResult.memory, description.name);
+    }
+
+
+    std::optional<robot::PlatformState>
+    RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+                                       const std::string& name) const
+    {
+        std::optional<robot::PlatformState> platformState;
+
+        // clang-format off
+        const armem::wm::CoreSegment& coreSegment = memory
+                .getCoreSegment(properties.proprioceptionCoreSegment);
+        // clang-format on
+
+        coreSegment.forEachEntity(
+            [&platformState](const wm::Entity& entity)
+            {
+                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+                const auto proprioception =
+                    tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+                ARMARX_CHECK(proprioception.has_value());
+
+                platformState = robot::PlatformState(); // initialize optional
+                fromAron(proprioception->platform, platformState.value());
+            });
 
         return platformState;
     }
 
+    inline RobotReader::Hand
+    fromHandName(const std::string& name)
+    {
+        if (name == "Left")
+        {
+            return RobotReader::Hand::Left;
+        }
+
+        if (name == "Right")
+        {
+            return RobotReader::Hand::Right;
+        }
+
+        throw LocalException("Unknown hand name `" + name + "`!");
+    }
+
+    std::map<RobotReader::Hand, robot::ForceTorque>
+    RobotReader::getForceTorque(const armarx::armem::wm::Memory& memory,
+                                const std::string& name) const
+    {
+        std::map<RobotReader::Hand, robot::ForceTorque> forceTorques;
+
+        // clang-format off
+        const armem::wm::CoreSegment& coreSegment = memory
+                .getCoreSegment(properties.proprioceptionCoreSegment);
+        // clang-format on
+
+        coreSegment.forEachEntity(
+            [&forceTorques](const wm::Entity& entity)
+            {
+                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+                const auto proprioception =
+                    tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+                ARMARX_CHECK(proprioception.has_value());
+
+
+                for (const auto& [handName, dtoFt] : proprioception->forceTorque)
+                {
+                    robot::ForceTorque forceTorque;
+                    fromAron(dtoFt, forceTorque);
+
+                    const auto hand = fromHandName(handName);
+                    forceTorques.emplace(hand, forceTorque);
+                }
+            });
+
+        return forceTorques;
+    }
+
+    std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+    RobotReader::getForceTorques(const armarx::armem::wm::Memory& memory,
+                                 const std::string& name) const
+    {
+        std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>> forceTorques;
+
+        // clang-format off
+        const armem::wm::CoreSegment& coreSegment = memory
+                .getCoreSegment(properties.proprioceptionCoreSegment);
+        // clang-format on
+
+        coreSegment.forEachEntity(
+            [&forceTorques](const wm::Entity& entity)
+            {
+                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+                const auto proprioception =
+                    tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+                ARMARX_CHECK(proprioception.has_value());
+
+
+                for (const auto& [handName, dtoFt] : proprioception->forceTorque)
+                {
+                    robot::ForceTorque forceTorque;
+                    fromAron(dtoFt, forceTorque);
+
+                    const auto hand = fromHandName(handName);
+                    forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
+                }
+            });
+
+        return forceTorques;
+    }
 
 
     std::optional<robot::RobotDescription>
@@ -382,11 +610,9 @@ namespace armarx::armem::robot_state
         // clang-format on
 
         const armem::wm::EntityInstance* instance = nullptr;
-        providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
-        {
-            instance = &i;
-        });
-        if (!instance)
+        providerSegment.forEachInstance([&instance](const wm::EntityInstance& i)
+                                        { instance = &i; });
+        if (instance == nullptr)
         {
             ARMARX_WARNING << "No entity snapshots found";
             return std::nullopt;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index e24bc65fa8a9ca14213bfe79007fc8519f02d176..20cafbd2225160c7b9abacc2d73b981f60260e34 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -26,10 +26,8 @@
 
 #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
 #include <RobotAPI/libraries/armem/client/Reader.h>
-
 #include <RobotAPI/libraries/armem_robot/client/interfaces.h>
 #include <RobotAPI/libraries/armem_robot/types.h>
-
 #include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
 
 
@@ -59,15 +57,21 @@ namespace armarx::armem::robot_state
                          const armem::Time& timestamp) override;
 
         std::optional<robot::RobotDescription> queryDescription(const std::string& name,
-                const armem::Time& timestamp);
+                                                                const armem::Time& timestamp);
 
         std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
-                const armem::Time& timestamp);
+                                                    const armem::Time& timestamp);
 
         std::optional<robot::RobotState::JointMap>
         queryJointState(const robot::RobotDescription& description,
                         const armem::Time& timestamp) const;
 
+        using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
+        
+        JointTrajectory
+        queryJointStates(const robot::RobotDescription& description,
+                        const armem::Time& begin, const armem::Time& end) const;
+
         std::optional<robot::RobotState::Pose>
         queryGlobalPose(const robot::RobotDescription& description,
                         const armem::Time& timestamp) const;
@@ -76,25 +80,52 @@ namespace armarx::armem::robot_state
         queryPlatformState(const robot::RobotDescription& description,
                            const armem::Time& timestamp) const;
 
+
+        enum class Hand
+        {
+            Left,
+            Right
+        };
+
+        std::optional<std::map<Hand, robot::ForceTorque>>
+        queryForceTorque(const robot::RobotDescription& description,
+                         const armem::Time& timestamp) const;
+
+        std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
+        queryForceTorques(const robot::RobotDescription& description,
+                          const armem::Time& start,
+                          const armem::Time& end) const;
+
     private:
         std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
-                const std::string& name) const;
+                                                       const std::string& name) const;
 
         std::optional<robot::RobotDescription>
         getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
-        
+
         std::optional<robot::RobotState::JointMap>
         getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
+        JointTrajectory
+        getRobotJointStates(const armarx::armem::wm::Memory& memory,
+                                    const std::string& name) const;
+
         std::optional<robot::PlatformState>
-        getRobotPlatformState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+        getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+                              const std::string& name) const;
+
+        std::map<RobotReader::Hand, robot::ForceTorque>
+        getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
+ 
+        std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+        getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
         struct Properties
         {
-            std::string memoryName                = "RobotState";
-            std::string descriptionCoreSegment    = "Description";
-            std::string localizationCoreSegment   = "Localization";
+            std::string memoryName = "RobotState";
+            std::string descriptionCoreSegment = "Description";
+            std::string localizationCoreSegment = "Localization";
             std::string proprioceptionCoreSegment = "Proprioception";
         } properties;
 
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index c2ed636a93e6a1b8feb9331daf68812160a44ba8..9b35f938d421e0662452ea5d6f49be87390553d2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -78,7 +78,7 @@ namespace armarx::armem::server::robot_state::proprioception
             {
                 std::lock_guard lock{dataMutex};
                 queueSize = dataQueue.size();
-                if (dataQueue.size() >= properties.memoryBatchSize)
+                if (!dataQueue.empty())
                 {
                     std::swap(batch, dataQueue);
                 }
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 10c5a8b9e4c52c2a45198d62f7661cda042ba8a4..1f66d5a7010e7c925513ab0463269261251ca434 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -84,7 +84,6 @@ namespace armarx::armem::server::robot_state::proprioception
 
         struct Properties
         {
-            unsigned int memoryBatchSize = 50;
             armem::MemoryID robotUnitProviderID;
         };
         Properties properties;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
index 24d31001590a87cb976f6b97f362c1262e7da288..0cd870ab77edc1a72f6cb5c90d8b820bde30b05e 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
@@ -78,6 +78,7 @@ namespace armarx::aron
         virtual std::string getName() const = 0;
 
     protected:
+
         // members
         const Descriptor descriptor;
         const Path path;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
index 5e92b40922ac3750aec89feb38d75ce522a966da..1838e7ac5178065c4e673a46f5d6cc1f1a6e7d4b 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
@@ -31,8 +31,8 @@
 #include <RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h>
 
 // ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
 #include <RobotAPI/libraries/aron/core/Descriptor.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
 
 namespace armarx::aron::datanavigator
 {
@@ -40,25 +40,26 @@ namespace armarx::aron::datanavigator
     typedef std::shared_ptr<NavigatorFactory> NavigatorFactoryPtr;
 
     class NavigatorFactory :
-        virtual public armarx::aron::NavigatorFactory<data::AronDataPtr, datanavigator::NavigatorPtr, data::Descriptor>
+        virtual public armarx::aron::
+            NavigatorFactory<data::AronDataPtr, datanavigator::NavigatorPtr, data::Descriptor>
     {
     public:
         NavigatorFactory() = default;
+        virtual ~NavigatorFactory() = default;
         virtual NavigatorPtr create(const data::AronDataPtr&, const Path&) const override;
         virtual NavigatorPtr createSpecific(const data::AronDataPtr&, const Path&) const override;
     };
 
     // Factories
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-    class upperType##NavigatorFactory : \
-        virtual public NavigatorFactory \
-    { \
-    public: \
-        upperType##NavigatorFactory() = default; \
+#define RUN_ARON_MACRO(upperType, lowerType, capsType)                                             \
+    class upperType##NavigatorFactory : virtual public NavigatorFactory                            \
+    {                                                                                              \
+    public:                                                                                        \
+        upperType##NavigatorFactory() = default;                                                   \
         virtual NavigatorPtr createSpecific(const data::AronDataPtr&, const Path&) const override; \
     };
 
     HANDLE_ALL_ARON_DATA
 #undef RUN_ARON_MACRO
 
-}
+} // namespace armarx::aron::datanavigator
diff --git a/source/RobotAPI/libraries/diffik/DiffIKProvider.h b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
index 2e3b0967afe3971e25c94a4d57f1b5a10b9ae638..b65a63b9a8c2e80cc4312afbe859c6d64e8f5f73 100644
--- a/source/RobotAPI/libraries/diffik/DiffIKProvider.h
+++ b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
@@ -39,13 +39,13 @@ namespace armarx
         Eigen::VectorXf jointValues;
 
     };
-    
+
     class DiffIKProvider
     {
     public:
         virtual DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose) = 0;
         virtual DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) = 0;
-   
+
         virtual ~DiffIKProvider() = default;
     };
 }
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
index 6d76464e94326b8e864ace29ec294d10aff65402..3e6cdff5c28a7a27494e222ce78517abe3d8a5f5 100644
--- a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
@@ -392,7 +392,7 @@ void GraspTrajectory::writeToFile(const std::string& filename)
 
 GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const grasping::GraspCandidatePtr& cnd)
 {
-    std::string packageName = "Armar6Skills";// cnd->executionHints->graspTrajectoryPackage;
+    std::string packageName = "armar6_skills";// cnd->executionHints->graspTrajectoryPackage;
     armarx::CMakePackageFinder finder(packageName);
     std::string dataDir = finder.getDataDir() + "/" + packageName;
     return GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/" + cnd->executionHints->graspTrajectoryName + ".xml");