diff --git a/scenarios/ArVizExample/config/RobotToArVizApp.cfg b/scenarios/ArVizExample/config/RobotToArVizApp.cfg
index 4f4ea65faf5690963ba6e1ef3a2970d18dcd6563..a38badd327185c65c1cdbb4b19b5534ab438b0e4 100644
--- a/scenarios/ArVizExample/config/RobotToArVizApp.cfg
+++ b/scenarios/ArVizExample/config/RobotToArVizApp.cfg
@@ -143,6 +143,14 @@
 # ArmarX.RobotToArViz.ObjectName = ""
 
 
+# ArmarX.RobotToArViz.RemoteGuiName:  Name of the remote gui provider
+#  Attributes:
+#  - Default:            RemoteGuiProvider
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.RobotToArViz.RemoteGuiName = RemoteGuiProvider
+
+
 # ArmarX.RobotToArViz.RemoteStateComponentName:  Name of the robot state component
 #  Attributes:
 #  - Default:            RobotStateComponent
@@ -151,6 +159,15 @@
 # ArmarX.RobotToArViz.RemoteStateComponentName = RobotStateComponent
 
 
+# ArmarX.RobotToArViz.ShowRobotNodeFrames:  If true, show frames of robot nodes (can be changed in RemoteGui).
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotToArViz.ShowRobotNodeFrames = false
+
+
 # ArmarX.RobotToArViz.updateFrequency:  Target number of updates per second.
 #  Attributes:
 #  - Default:            100
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 76331fc89b2352b1dd7130425f1193d370d5e818..7e1ec4053f83a4d33bb323bd129ff87f0d7b703a 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -5,12 +5,16 @@ set(COMPONENT_LIBS
     RobotAPICore
     RobotAPIInterfaces
     RobotAPIArmarXObjects
+    RobotStatechartHelpers  # For RobotNameHelper, used by RobotHand
     boost_iostreams #compression
 )
 
 set(SOURCES
 
+    Client/Elements.cpp
     Client/elements/Mesh.cpp
+    Client/elements/Robot.cpp
+    Client/elements/RobotHand.cpp
 
     Coin/ElementVisualizer.cpp
 
@@ -68,6 +72,8 @@ set(HEADERS
     Client/elements/Mesh.h
     Client/elements/MeshCGALExtensions.h
     Client/elements/PointCloud.h
+    Client/elements/Robot.h
+    Client/elements/RobotHand.h
 
     Client/elements/point_cloud_type_traits.hpp
 
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57e7a5c74665b112f0999c37be4bd2ff37be5687
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -0,0 +1,26 @@
+#include "Elements.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+
+namespace armarx::viz
+{
+
+    const std::string Object::DefaultObjectsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
+
+    Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage)
+    {
+        return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage);
+    }
+
+    Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage)
+    {
+        ObjectInfo info(objectsPackage, "", objectID);
+        armarx::PackageFileLocation file = info.simoxXML();
+        return this->file(file.package, file.relativePath);
+    }
+}
+
+
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 1f6b3f794deea5fdb7677501baa7a092d70e3735..a976534bfd392f4579d137b9ecdcd776ec738622 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -17,8 +17,10 @@
 
 #include "Color.h"
 #include "elements/ElementOps.h"
-#include "elements/PointCloud.h"
 #include "elements/Mesh.h"
+#include "elements/PointCloud.h"
+#include "elements/Robot.h"
+//#include "elements/RobotHand.h"  // Not included by default (exposes additional headers).
 
 // The has_member macro causes compile errors if *any* other header uses
 // the identifier has_member. Boost.Thread does, so this causes compile
@@ -34,6 +36,11 @@ namespace Eigen
     using Hyperplane3f = Hyperplane<float, 3>;
 }
 
+namespace armarx
+{
+    ///@see <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+    class ObjectID;
+}
 
 namespace armarx::viz
 {
@@ -443,65 +450,11 @@ namespace armarx::viz
     };
 
 
-    class Robot : public ElementOps<Robot, data::ElementRobot>
-    {
-    public:
-        using ElementOps::ElementOps;
-
-        Robot& file(std::string const& project, std::string const& filename)
-        {
-            data_->project = project;
-            data_->filename = filename;
-
-            return *this;
-        }
-
-        Robot& useCollisionModel()
-        {
-            data_->drawStyle |= data::ModelDrawStyle::COLLISION;
-
-            return *this;
-        }
-
-        Robot& useFullModel()
-        {
-            data_->drawStyle &= ~data::ModelDrawStyle::COLLISION;
-
-            return *this;
-        }
-
-        Robot& overrideColor(Color c)
-        {
-            data_->drawStyle |= data::ModelDrawStyle::OVERRIDE_COLOR;
-
-            return color(c);
-        }
-
-        Robot& useOriginalColor()
-        {
-            data_->drawStyle &= ~data::ModelDrawStyle::OVERRIDE_COLOR;
-
-            return *this;
-        }
-
-        Robot& joint(std::string const& name, float value)
-        {
-            data_->jointValues[name] = value;
-
-            return *this;
-        }
-
-        Robot& joints(std::map<std::string, float> const& values)
-        {
-            data_->jointValues = values;
-
-            return *this;
-        }
-    };
-
-
     class Object : public ElementOps<Object, data::ElementObject>
     {
+    private:
+        static const std::string DefaultObjectsPackage;
+
     public:
         using ElementOps::ElementOps;
 
@@ -513,6 +466,16 @@ namespace armarx::viz
             return *this;
         }
 
+        /**
+         * @brief Set the file so it could be found using `armarx::ObjectFinder` (also on remote machine).
+         * @param objectID The object ID, see <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+         * @param objectsPackage The objects package ("ArmarXObjects" by default)
+         * @see <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+         */
+        Object& fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
+        Object& fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
+
+
         Object& useCollisionModel()
         {
             data_->drawStyle |= data::ModelDrawStyle::COLLISION;
@@ -540,6 +503,7 @@ namespace armarx::viz
 
             return *this;
         }
+
     };
 
 }
diff --git a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
index e7b8f0542fecdb99ec55599c9fdf4a371a3c4065..4f7c222f5c8ebb3f6a5f65062033218e058a8ddb 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
@@ -140,6 +140,33 @@ namespace armarx::viz
             return *static_cast<DerivedT*>(this);
         }
 
+        DerivedT& hide()
+        {
+            data_->flags |= data::ElementFlags::HIDDEN;
+
+            return *static_cast<DerivedT*>(this);
+        }
+
+        DerivedT& show()
+        {
+            data_->flags &= ~data::ElementFlags::HIDDEN;
+
+            return *static_cast<DerivedT*>(this);
+        }
+
+        DerivedT& visible(bool visible)
+        {
+            if (visible)
+            {
+                return show();
+            }
+            else
+            {
+                return hide();
+            }
+        }
+
+
         IceInternal::Handle<ElementT> data_;
     };
 
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp b/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b77725be1ca0ee3dafdc24a5e1513275aebe796b
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp
@@ -0,0 +1,8 @@
+#include "Robot.h"
+
+
+namespace armarx::viz
+{
+
+
+}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Robot.h b/source/RobotAPI/components/ArViz/Client/elements/Robot.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c90e0209498ae0d6c37d80a19905df32f7565d9
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Robot.h
@@ -0,0 +1,66 @@
+#pragma once
+
+#include "ElementOps.h"
+
+
+namespace armarx::viz
+{
+
+    class Robot : public ElementOps<Robot, data::ElementRobot>
+    {
+    public:
+        using ElementOps::ElementOps;
+
+        Robot& file(std::string const& project, std::string const& filename)
+        {
+            data_->project = project;
+            data_->filename = filename;
+
+            return *this;
+        }
+
+        Robot& useCollisionModel()
+        {
+            data_->drawStyle |= data::ModelDrawStyle::COLLISION;
+
+            return *this;
+        }
+
+        Robot& useFullModel()
+        {
+            data_->drawStyle &= ~data::ModelDrawStyle::COLLISION;
+
+            return *this;
+        }
+
+        Robot& overrideColor(Color c)
+        {
+            data_->drawStyle |= data::ModelDrawStyle::OVERRIDE_COLOR;
+
+            return color(c);
+        }
+
+        Robot& useOriginalColor()
+        {
+            data_->drawStyle &= ~data::ModelDrawStyle::OVERRIDE_COLOR;
+
+            return *this;
+        }
+
+        Robot& joint(std::string const& name, float value)
+        {
+            data_->jointValues[name] = value;
+
+            return *this;
+        }
+
+        Robot& joints(std::map<std::string, float> const& values)
+        {
+            data_->jointValues = values;
+
+            return *this;
+        }
+
+    };
+
+}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ea0966ff156625fedc0c1287771a32e29b2cb9e9
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp
@@ -0,0 +1,45 @@
+#include "RobotHand.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::viz
+{
+
+    RobotHand& RobotHand::fileBySide(const std::string& side, RobotInfoNodePtr robotInfo)
+    {
+        ARMARX_CHECK_NOT_NULL(robotInfo);
+        RobotNameHelper nh(robotInfo, nullptr);
+        return this->fileBySide(side, nh);
+    }
+
+    RobotHand& RobotHand::fileBySide(const std::string& side, const RobotNameHelper& nameHelper)
+    {
+        RobotNameHelper::Arm arm = nameHelper.getArm(side);
+        return this->fileByArm(arm);
+    }
+
+    RobotHand& RobotHand::fileByArm(const RobotNameHelper::Arm& arm)
+    {
+        this->arm = arm;
+        this->file(arm.getHandModelPackage(), arm.getHandModelPath());
+        return *this;
+    }
+
+    RobotHand& RobotHand::fileByArm(const RobotNameHelper::RobotArm& arm)
+    {
+        return this->fileByArm(arm.getArm());
+    }
+
+    RobotHand& RobotHand::tcpPose(const Eigen::Matrix4f& tcpPose, VirtualRobot::RobotPtr robot)
+    {
+        ARMARX_CHECK(arm) << "Set RobotHand::side() before setting the TCP pose.";
+        RobotNameHelper::RobotArm robotArm = arm->addRobot(robot);
+        this->pose(tcpPose * robotArm.getTcp2HandRootTransform());
+        return *this;
+    }
+}
+
+
+
+
diff --git a/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h
new file mode 100644
index 0000000000000000000000000000000000000000..0a6e7d4a44f5343a4143c338b66131bf7cfe1363
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include <optional>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
+
+#include "Robot.h"
+
+
+namespace armarx::viz
+{
+
+    /**
+     * @brief Left or right hand of a robot.
+     */
+    class RobotHand : public Robot
+    {
+    public:
+
+        using Robot::Robot;
+
+        /**
+         * @brief Set the robot file according the desired side.
+         * @param side The side ("Left" or "Right", see `RobotNameHelper::LocationLeft, LocationRight`)
+         * @param robotInfo
+         *  The robot info.
+         *  You can get it from a RobotStateComponent: `robotStateComponent->getRobotInfo()`.
+         *  If you do this in a loop, you should get the robot info just once before the loop.
+         */
+        RobotHand& fileBySide(const std::string& side, RobotInfoNodePtr robotInfo);
+        RobotHand& fileBySide(const std::string& side, const RobotNameHelper& nameHelper);
+        RobotHand& fileByArm(const RobotNameHelper::Arm& arm);
+        RobotHand& fileByArm(const RobotNameHelper::RobotArm& arm);
+
+
+        /**
+         * @brief Set the pose of `robotViz` according to the given TCP pose.
+         *
+         * You must specify the side beforehand using `fileBySide()`.
+         */
+        RobotHand& tcpPose(const Eigen::Matrix4f& tcpPose, VirtualRobot::RobotPtr robot);
+
+
+        /// The arm name helper. Set by `fileBySide()`.
+        std::optional<RobotNameHelper::Arm> arm;
+
+    };
+
+}
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
index 8c3f524fb4baa5faca577eb0651d561bc302ac58..9be0fa5b5c44d5256ae0adb4cc0c6a74d5dfe619 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
@@ -7,6 +7,8 @@
 
 #include <memory>
 
+#include <RobotAPI/interface/ArViz/Elements.h>
+
 namespace armarx::viz::data
 {
     class Element;
@@ -27,6 +29,7 @@ namespace armarx::viz::coin
         SoTransform* transform;
         SoMaterial* material;
         bool wasUpdated = true;
+        bool visible = true;
     };
 
     class ElementVisualizer
@@ -50,6 +53,12 @@ namespace armarx::viz::coin
         TypedElementVisualization()
         {
             node = new NodeType;
+            node->ref();
+        }
+
+        ~TypedElementVisualization()
+        {
+            node->unref();
         }
 
         NodeType* node;
@@ -62,6 +71,7 @@ namespace armarx::viz::coin
         using DataType = DataT;
         using ElementType = typename DataType::ElementType;
 
+
         DataType* createDerived() final
         {
             DataType* result = new DataType;
@@ -73,8 +83,24 @@ namespace armarx::viz::coin
         {
             auto const& element = static_cast<ElementType const&>(element_);
             auto* data = dynamic_cast<DataType*>(data_);
+
             if (data)
             {
+                bool hidden = (element.flags & armarx::viz::data::ElementFlags::HIDDEN);
+                if (hidden)
+                {
+                    if (data->visible)
+                    {
+                        data->separator->removeChild(data->node);
+                        data->visible = false;
+                    }
+                }
+                else if (!data->visible)
+                {
+                    data->visible = true;
+                    data->separator->addChild(data->node);
+                }
+
                 // We want to call  update with the correctly downcasted arguments
                 return data->update(element);
             }
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index bf0ae91ddc2dd48cd7368254c58cb80258d04d3f..e02312f388610108db2caccf4d81e31a80000a06 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -90,6 +90,7 @@ namespace armarx::viz::coin
     bool VisualizationRobot::update(ElementType const& element)
     {
         IceUtil::Time time_start = IceUtil::Time::now();
+        (void) time_start;
 
         bool robotChanged = loaded.project != element.project || loaded.filename != element.filename;
         if (robotChanged)
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 9043aa8ae83977c97f64cc5bf4ac1978e9e06aeb..6cb09640398b08c7bae7202b9db8843ed908e83d 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -69,6 +69,9 @@ namespace armarx
                            .color(viz::Color::red())
                            .size(Eigen::Vector3f(100.0f, 100.0f, 100.0f));
 
+            bool toggleVisibility = (static_cast<int>(timeInSeconds) % 2 == 0);
+            box.visible(toggleVisibility);
+
             layer.add(box);
         }
         {
@@ -125,6 +128,7 @@ namespace armarx
             Eigen::Vector3f direction = dirRot * Eigen::Vector3f::UnitX();
             arrow.direction(direction);
 
+
             Eigen::Vector3f pos = Eigen::Vector3f::Zero();
             pos.z() = +300.0f;
             pos.x() = -500.0f;
@@ -133,6 +137,8 @@ namespace armarx
 
             layer.add(arrow);
         }
+
+
     }
 
 
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
index b7d21d33b49202aa16928be602d306ed522ad9a1..22ff30654400ffb349b013b3d58e69b007f8ed18 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
@@ -76,6 +76,11 @@ namespace armarx
         if (data.leftTrigger > 0)
         {
             emergencyStop->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+            return;
+        }
+        else if (data.startButton)
+        {
+            emergencyStop->setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
         }
         //scales are for the robot axis
         if (data.rightTrigger > 0)
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index 1c4733b05d4e90d5a313026aeaaabd738932b024..65fa2f6ddf1046e8d578798cbb94023110a4907f 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -269,6 +269,7 @@ namespace armarx
         Eigen::VectorXf optimizationGradient(unsigned int id) override
         {
             int size = nodeSet->getSize();
+            (void) size;
 
             Eigen::MatrixXf J = ik->getJacobianMatrix(tcp);//.block(0, 0, 3, size);
             //Eigen::Vector3f d = eef->getGlobalPose().block<3,1>(0,3) - target;
@@ -326,6 +327,7 @@ namespace armarx
         Eigen::Vector3f shoulder_L = sho1_L->getPositionInRootFrame();
 
         Eigen::Vector3f offset = (shoulder_R + shoulder_L) / 2;
+        (void) offset;
 
         NaturalIK ik_R("Right", shoulder_R, 1);
         NaturalIK ik_L("Left", shoulder_L, 1);
diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index 867170f06d57371fee6e25cda2f28ea4dffdda79..f369dd0247620277b719e59891bbff9c88d13e6e 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
@@ -5,6 +5,7 @@ set(COMPONENT_LIBS
     ArmarXCore ArmarXCoreInterfaces
     ArmarXGuiComponentPlugins
     RobotAPIArmarXObjects RobotAPIComponentPlugins
+    ArViz
 
     ${PROJECT_NAME}Interfaces
 )
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
index ae8531edacea24b495fac67c39fd50f51a927835..af808d1de3634821a761e2ae4bb3183aa1e2bcfd 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -113,6 +113,25 @@ namespace armarx::objpose
         objpose::fromIce(provided.localOOBB, localOOBB);
     }
 
+
+    std::optional<simox::OrientedBoxf> ObjectPose::oobbRobot() const
+    {
+        if (localOOBB)
+        {
+            return localOOBB->transformed(objectPoseRobot);
+        }
+        return std::nullopt;
+    }
+
+    std::optional<simox::OrientedBoxf> ObjectPose::oobbGlobal() const
+    {
+        if (localOOBB)
+        {
+            return localOOBB->transformed(objectPoseGlobal);
+        }
+        return std::nullopt;
+    }
+
 }
 
 namespace armarx
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
index 78cbfbfc6bbe46bd0dc8c24995dfc33973f68e3b..d00e548b82bc9bdd72a46aeb997a6eb297e89ba4 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
@@ -54,7 +54,15 @@ namespace armarx::objpose
         IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);
 
         /// Object bounding box in object's local coordinate frame.
+        /// @see oobbRobot(), oobbGlobal()
         std::optional<simox::OrientedBoxf> localOOBB;
+
+
+        /// Get the OOBB in the robot frame (according to `objectPoseRobot`).
+        std::optional<simox::OrientedBoxf> oobbRobot() const;
+        /// Get the OOBB in the global frame (according to `objectPoseGlobal`).
+        std::optional<simox::OrientedBoxf> oobbGlobal() const;
+
     };
     using ObjectPoseSeq = std::vector<ObjectPose>;
 
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 8f24af53ed2dca19b6c8b273da70842bbc26dc48..fda9571084893b61eef4cdde94a599720bc3fd34 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -53,6 +53,9 @@ namespace armarx
         defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects.");
         defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
         defs->optional(visu.alpha, "visu.alpha", "Alpha of objects (1 = solid, 0 = transparent).");
+        defs->optional(visu.oobbs, "visu.oobbs", "Enable showing oriented bounding boxes.");
+        defs->optional(visu.objectFrames, "visu.objectFrames", "Enable showing object frames.");
+        defs->optional(visu.objectFramesScale, "visu.objectFramesScale", "Scaling of object frames.");
         defs->optional(calibration.robotNode, "calibration.robotNode", "Robot node which can be calibrated");
         defs->optional(calibration.offset, "calibration.offset", "Offset for the node to be calibrated");
 
@@ -100,6 +103,14 @@ namespace armarx
         tab.visuAlpha.setRange(0, 1.0);
         tab.visuAlpha.setValue(visu.alpha);
         tab.visuOOBBs.setValue(visu.oobbs);
+        tab.visuObjectFrames.setValue(visu.objectFrames);
+        {
+            float max = 10000;
+            tab.visuObjectFramesScale.setRange(0, max);
+            tab.visuObjectFramesScale.setDecimals(2);
+            tab.visuObjectFramesScale.setSteps(int(10 * max));
+            tab.visuObjectFramesScale.setValue(visu.objectFramesScale);
+        }
 
         GroupBox visuGroup;
         {
@@ -109,10 +120,13 @@ namespace armarx
             row++;
             grid.add(Label("Global Frame"), {row, 0}).add(tab.visuInGlobalFrame, {row, 1});
             row++;
-            grid.add(Label("Alpha"), {row, 0}).add(tab.visuAlpha, {row, 1});
+            grid.add(Label("Alpha"), {row, 0}).add(tab.visuAlpha, {row, 1}, {1, 3});
             row++;
             grid.add(Label("OOBB"), {row, 0}).add(tab.visuOOBBs, {row, 1});
             row++;
+            grid.add(Label("Object Frames"), {row, 0}).add(tab.visuObjectFrames, {row, 1});
+            grid.add(Label("Scale:"), {row, 2}).add(tab.visuObjectFramesScale, {row, 3});
+            row++;
 
             visuGroup.setLabel("Visualization");
             visuGroup.addChild(grid);
@@ -131,6 +145,8 @@ namespace armarx
         visu.inGlobalFrame = tab.visuInGlobalFrame.getValue();
         visu.alpha = tab.visuAlpha.getValue();
         visu.oobbs = tab.visuOOBBs.getValue();
+        visu.objectFrames = tab.visuObjectFrames.getValue();
+        visu.objectFramesScale = tab.visuObjectFramesScale.getValue();
     }
 
 
@@ -372,26 +388,23 @@ namespace armarx
             const armarx::ObjectID id = objectPose.objectID;
             std::string key = id.str();
 
-            std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
-
             Eigen::Matrix4f pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
-
-            if (objectInfo)
             {
-                viz::Object object = viz::Object(key)
-                                     .file(objectInfo->package(), objectInfo->simoxXML().relativePath)
-                                     .pose(pose);
+                viz::Object object = viz::Object(key).pose(pose);
+                if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+                {
+                    object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
+                }
+                else
+                {
+                    object.fileByObjectFinder(id);
+                }
                 if (visu.alpha < 1)
                 {
                     object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
                 }
-
                 layer.add(object);
             }
-            else
-            {
-                ARMARX_WARNING << "Cannot visualize object '" << key << "'.";
-            }
 
             if (visu.oobbs && objectPose.localOOBB)
             {
@@ -400,6 +413,10 @@ namespace armarx
                           .pose(pose * simox::math::pose(oobb.center(), oobb.rotation()))
                           .size(oobb.dimensions()).color(simox::Color::lime(255, 64)));
             }
+            if (visu.objectFrames)
+            {
+                layer.add(viz::Pose(key + " Pose").pose(pose).scale(visu.objectFramesScale));
+            }
         }
 
         arviz.commit(layer);
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index c6271e22e5edcc83b1cb9d532da6405a31920d30..1e7197d3b068ad58fad6a01912b6f9e7d6b51a6f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -152,6 +152,8 @@ namespace armarx
             bool inGlobalFrame = true;
             float alpha = 1.0;
             bool oobbs = false;
+            bool objectFrames = false;
+            float objectFramesScale = 1.0;
         };
         Visu visu;
 
@@ -169,6 +171,8 @@ namespace armarx
             RemoteGui::Client::CheckBox visuInGlobalFrame;
             RemoteGui::Client::FloatSlider visuAlpha;
             RemoteGui::Client::CheckBox visuOOBBs;
+            RemoteGui::Client::CheckBox visuObjectFrames;
+            RemoteGui::Client::FloatSpinBox visuObjectFramesScale;
         };
         RemoteGuiTab tab;
     };
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
index 63311003da6f9911fad75c5faa154010e2280725..05ea121479788774bd7e63af534734f1c7720fb7 100644
--- a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
@@ -38,6 +38,8 @@ namespace armarx
 
         // defs->optional(updateFrequency, "updateFrequency", "Target number of updates per second.");
         defs->defineOptionalProperty("updateFrequency", updateFrequency, "Target number of updates per second.");
+        defs->optional(gui.useCollisionModel, "UseCollisionModel", "Use the collision model for visualization");
+
         defs->optional(gui.showRobotNodeFrames, "ShowRobotNodeFrames",
                        "If true, show frames of robot nodes (can be changed in RemoteGui).");
 
@@ -65,6 +67,11 @@ namespace armarx
         // Initialize robot visu element.
         this->robotViz = viz::Robot(robot->getName()).file("", robot->getFilename());
 
+        if (gui.useCollisionModel)
+        {
+            this->robotViz.useCollisionModel();
+        }
+
         createRemoteGuiTab();
         RemoteGui_startRunningTask();
 
@@ -98,6 +105,11 @@ namespace armarx
             root.add(Label("Show Robot Node Frames"), {row, 0});
             root.add(tab.showRobotNodeFrames, {row, 1});
             row += 1;
+
+            tab.useCollisionModel.setValue(gui.useCollisionModel);
+            root.add(Label("Use Collision Model"), {row, 0});
+            root.add(tab.useCollisionModel, {row, 1});
+            row += 1;
         }
         RemoteGui_createTab(getName(), root, &tab);
     }
@@ -109,6 +121,20 @@ namespace armarx
         {
             gui.showRobotNodeFrames = newShowRobotNodeFrames;
         }
+
+        bool newUseCollisionmodel = tab.useCollisionModel.getValue();
+        if (newUseCollisionmodel != gui.useCollisionModel)
+        {
+            gui.useCollisionModel = newUseCollisionmodel;
+            if (newUseCollisionmodel)
+            {
+                robotViz.useCollisionModel();
+            }
+            else
+            {
+                robotViz.useFullModel();
+            }
+        }
     }
 
 
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.h b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
index c289de35fdb7d98d92bb22b68fdeb93c6b91eb2c..ebf9ba01103e771bea9b667ef98a9352bbc562d1 100644
--- a/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
@@ -116,6 +116,8 @@ namespace armarx
         struct Tab : RemoteGui::Client::Tab
         {
             RemoteGui::Client::CheckBox showRobotNodeFrames;
+
+            RemoteGui::Client::CheckBox useCollisionModel;
             // Todo: add spin box for scale
         };
         Tab tab;
@@ -123,6 +125,9 @@ namespace armarx
         struct Gui
         {
             bool showRobotNodeFrames = false;
+
+            bool useCollisionModel = false;
+
         };
         Gui gui;
 
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 0994896f9a3510a8c4ce0241d981d0dad2bf03cc..8bcf8338726c29094ada9aa81cc74a69812b7150 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -322,6 +322,8 @@ namespace armarx::RobotUnitModule
 
         RobotUnitDataStreaming::DataStreamingDescription result;
         DataStreamingEntry& streamingEntry = rtDataStreamingEntry[receiver];
+        getProperty(streamingEntry.rtStreamMaxClientErrors,
+                    "RTLogging_StreamingDataMaxClientConnectionFailures");
 
         ARMARX_INFO << "start data streaming to " << receiver->ice_getIdentity().name
                     << ". Values: " << config.loggingNames;
@@ -870,7 +872,7 @@ namespace armarx::RobotUnitModule
         {
             ARMARX_TRACE;
             ++connectionFailures;
-            if (connectionFailures > 10) ///TODO make property
+            if (connectionFailures > rtStreamMaxClientErrors)
             {
                 stopStreaming = true;
                 ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index 2df9d88d2d8d497d12985fca320c95a8ea0e5576..6271ef1784a4cd79e8e3b37addcb4e679e946bc2 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -69,6 +69,10 @@ namespace armarx::RobotUnitModule
                 "RTLogging_KeepIterationsForMs", 60 * 1000,
                 "All logging data (SensorValues, ControlTargets, Messages) is keept for this duration "
                 "and can be dunped in case of an error.");
+
+            defineOptionalProperty<std::size_t>(
+                "RTLogging_StreamingDataMaxClientConnectionFailures", 10,
+                "If sending data to a client fails this often, the client is removed from the list");
         }
     };
 
@@ -223,6 +227,7 @@ namespace armarx::RobotUnitModule
 
             bool        onlyNewestFrame    = false;
             std::size_t connectionFailures = 0;
+            std::size_t rtStreamMaxClientErrors = 0;
 
             std::vector<std::vector<OutVal>>              sensDevs;
             std::vector<std::vector<std::vector<OutVal>>> ctrlDevs;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index 16d76a7b7bba97433af9a97e26c4fe66c387b051..49e77ca8b8afb43e7308a8e53316c297fb283e4e 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -139,7 +139,6 @@ namespace armarx::introspection
     make_def_for_build_in_ice_type(Ice::Float);
     make_def_for_build_in_ice_type(Ice::Double);
 #undef make_def_for_build_in_ice_type
-
 }
 //Eigen::Vector3f
 namespace armarx::introspection
@@ -202,7 +201,6 @@ namespace armarx::introspection
     make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
     make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
 #undef make_DataFieldsInfo_for_eigen_vector
-
 }
 //Eigen::Matrix4f
 namespace armarx::introspection
@@ -308,3 +306,106 @@ namespace armarx::introspection
             const std::string& agent);
     };
 }
+//std::array
+namespace armarx::introspection
+{
+    template<class T, std::size_t N>
+    struct DataFieldsInfo <
+        std::array<T, N>,
+    std::void_t<decltype(&DataFieldsInfo<T>::GetFieldNames)>
+> : DataFieldsInfoBase<std::array<T, N>>
+    {
+        using sub_t = DataFieldsInfo<T>;
+        static std::size_t GetNumberOfFields()
+        {
+            return N * sub_t::GetNumberOfFields();
+        }
+
+        template<class OT>
+        static void GetDataFieldAs(std::size_t i, const std::array<T, N>& field, OT& out)
+        {
+            ARMARX_CHECK_LESS(i, GetNumberOfFields());
+            sub_t::GetDataFieldAs(i % N, field.at(i / N), out);
+        }
+
+        static const std::type_info& GetDataFieldType(std::size_t i)
+        {
+            return sub_t::GetDataFieldType(i % N);
+        }
+        static std::vector<std::string> GetFieldNames()
+        {
+            const auto sub_names = sub_t::GetFieldNames();
+            std::vector<std::string> result;
+            result.reserve(N * sub_names.size());
+            for (std::size_t i = 0; i <  N; ++i)
+            {
+                const std::string pre = "element_" + std::to_string(i) + '.';
+                for (const auto& name : sub_names)
+                {
+                    result.emplace_back(pre + name);
+                }
+            }
+            return result;
+        }
+        static std::map<std::string, VariantBasePtr> ToVariants(
+            const std::array<T, N>& value,
+            const std::string& name,
+            const IceUtil::Time& timestamp,
+            const std::string& frame = "",
+            const std::string& agent = "")
+        {
+            std::map<std::string, VariantBasePtr> result;
+            for (std::size_t i = 0; i <  N; ++i)
+            {
+                const std::string pre = "element_" + std::to_string(i) + '.';
+                for (const auto& [k, v] : sub_t::ToVariants(value.at(i), name, timestamp, frame, agent))
+                {
+                    result[pre + k] = v;
+                }
+            }
+            return result;
+        }
+    };
+}
+//basic integral types
+namespace armarx::introspection
+{
+#define make_def_for_type_mapped_to_long(Type)                                          \
+    template<>                                                                          \
+    struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type>                        \
+    {                                                                                   \
+        using DataFieldsInfoBase<Type>::GetDataFieldAs;                                 \
+        static std::size_t GetNumberOfFields()                                          \
+        {                                                                               \
+            return 1;                                                                   \
+        }                                                                               \
+        static void GetDataFieldAs(std::size_t i, const Type& field, std::string& out)  \
+        {                                                                               \
+            ARMARX_CHECK_EQUAL(i, 0);                                                   \
+            out = to_string(field);                                                     \
+        }                                                                               \
+        static void GetDataFieldAs(std::size_t i, const Type& field, Ice::Long& out)    \
+        {                                                                               \
+            ARMARX_CHECK_EQUAL(i, 0);                                                   \
+            out = field;                                                                \
+        }                                                                               \
+        static const std::type_info& GetDataFieldType(std::size_t i)                    \
+        {                                                                               \
+            return typeid(Type);                                                        \
+        }                                                                               \
+        static std::map<std::string, VariantBasePtr> ToVariants(                        \
+                const Type& value,                                                      \
+                const std::string& name,                                                \
+                const IceUtil::Time& timestamp,                                         \
+                const std::string& frame = "",                                          \
+                const std::string& agent = "")                                          \
+        {                                                                               \
+            ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())                     \
+                    << "There is no framed version for build in ice types";             \
+            return {{name, {new TimedVariant(value, timestamp)}}};                      \
+        }                                                                               \
+    }
+    make_def_for_type_mapped_to_long(std::uint16_t);
+    make_def_for_type_mapped_to_long(std::uint32_t);
+#undef make_def_for_type_mapped_to_long
+}
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui b/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
index 00e1d435e4567cca000291e074d2f566d397d144..b5dc5e5dd125a004758a383ad4c431517bd3f1d2 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
@@ -102,10 +102,16 @@
         </layout>
        </item>
        <item>
-        <widget class="QGroupBox" name="groupBox">
+        <widget class="QGroupBox" name="layerInfoTreeGroupBox">
          <property name="title">
           <string>Layer Information</string>
          </property>
+         <property name="checkable">
+          <bool>true</bool>
+         </property>
+         <property name="checked">
+          <bool>false</bool>
+         </property>
          <layout class="QVBoxLayout" name="verticalLayout_6">
           <property name="leftMargin">
            <number>3</number>
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index c893f73b6927c28cbb6db2641e890155989d6235..006be56ad97d2f3f9fa7b1947df08f002bb52d09 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -32,7 +32,7 @@
 
 #include <ArmarXCore/observers/variant/Variant.h>
 
-#define ENABLE_INTROSPECTION 0
+#define ENABLE_INTROSPECTION 1
 
 
 namespace armarx
@@ -93,13 +93,17 @@ namespace armarx
         // Layer info tree.
 
         connect(widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
+
 #if ENABLE_INTROSPECTION
+        connect(widget.layerInfoTreeGroupBox, &QGroupBox::toggled, &layerInfoTree, &LayerInfoTree::setEnabled);
         connect(widget.defaultShowLimitSpinBox, qOverload<int>(&QSpinBox::valueChanged),
                 &layerInfoTree, &LayerInfoTree::setMaxElementCountDefault);
         layerInfoTree.setMaxElementCountDefault(widget.defaultShowLimitSpinBox->value());
 
         layerInfoTree.setWidget(widget.layerInfoTree);
+        layerInfoTree.setEnabled(widget.layerInfoTreeGroupBox->isChecked());
         layerInfoTree.registerVisualizerCallbacks(visualizer);
+
 #endif
 
 
diff --git a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
index aeecddbef7737399b08182c9e067f1ee8e01ebc8..3245ae66c52e75bc663f5dae8128a7d102a0c6af 100644
--- a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
@@ -72,6 +72,22 @@ namespace armarx
         this->maxElementCountDefault = max;
     }
 
+    void LayerInfoTree::setEnabled(bool value)
+    {
+        if (this->enabled != value)
+        {
+            this->enabled = value;
+            if (value)
+            {
+                update();
+            }
+            else
+            {
+                clearLayerElements();
+            }
+        }
+    }
+
     void LayerInfoTree::setSelectedLayer(viz::CoinLayerID id, viz::CoinLayer* layer)
     {
         this->layerID = id;
@@ -92,7 +108,20 @@ namespace armarx
 
     void LayerInfoTree::update()
     {
-        updateLayerElements(layer->elements);
+        if (enabled && layer)
+        {
+            updateLayerElements(layer->elements);
+        }
+    }
+
+
+    void LayerInfoTree::clearLayerElements()
+    {
+        if (widget)
+        {
+            widget->clear();
+            showMoreItem = nullptr;
+        }
     }
 
 
@@ -169,7 +198,8 @@ namespace armarx
         int index, const std::string& name, const viz::data::ElementPtr& element)
     {
         QTreeWidgetItem* item = new QTreeWidgetItem(QStringList { name.c_str(), getTypeName(element).c_str() });
-        item->setCheckState(0, Qt::CheckState::Unchecked);
+        // To be used when we can hide specific elements.
+        // item->setCheckState(0, Qt::CheckState::Unchecked);
         widget->insertTopLevelItem(index, item);
         return item;
     }
@@ -300,7 +330,8 @@ namespace armarx
     QTreeWidgetItem* LayerInfoTree::addJsonChild(QTreeWidgetItem* parent, const std::string& key)
     {
         QTreeWidgetItem* child = new QTreeWidgetItem(QStringList { key.c_str(), "" });
-        child->setCheckState(0, Qt::CheckState::Unchecked);
+        // To be used when we can actually change the values (enabled = use value from GUI).
+        // child->setCheckState(0, Qt::CheckState::Unchecked);
         parent->addChild(child);
 
         return child;
diff --git a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
index eeb169e5c4cf59b12d9e5721cbd9237d28a49a5c..a5289ac8b10aa7a7d184c3e4922c26fed5dabcc4 100644
--- a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
+++ b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
@@ -26,6 +26,13 @@ namespace armarx
         void registerVisualizerCallbacks(viz::CoinVisualizer& visualizer);
         void setMaxElementCountDefault(int max);
 
+        /**
+         * @brief Enable or disable the layer info tree.
+         *
+         * When enabled, the tree will be filled on the first update.
+         * When disabled, the tree is cleared and will not be updated.
+         */
+        void setEnabled(bool enabled);
 
         /// Set the selected layer.
         void setSelectedLayer(viz::CoinLayerID id, viz::CoinLayer* layer);
@@ -43,6 +50,8 @@ namespace armarx
 
     private:
 
+        void clearLayerElements();
+
         /**
          * @brief Updates the list of layer elements.
          *
@@ -102,10 +111,12 @@ namespace armarx
 
     private:
 
-        QTreeWidget* widget;
+        QTreeWidget* widget = nullptr;
         /// Connections to the current widget.
         QVector<QMetaObject::Connection> widgetConnections;
 
+        bool enabled = true;
+
         QTreeWidgetItem* showMoreItem = nullptr;
         int maxElementCountDefault = 25;
         int maxElementCount = maxElementCountDefault;
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f521730131e1f3e3a70e1c037d18381cd0a1f1cc
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \package    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
new file mode 100644
index 0000000000000000000000000000000000000000..095a535247b070c039a680cecb3031c951c750ca
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \package    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
new file mode 100644
index 0000000000000000000000000000000000000000..0b5b4e258a5db005fb5e2a666baa3f4073085a8c
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidget.ui
@@ -0,0 +1,2344 @@
+<?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
new file mode 100644
index 0000000000000000000000000000000000000000..92eb4c7083fde0208663cd7fc55b3de101fcb7ae
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.cpp
@@ -0,0 +1,419 @@
+/*
+ * 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
new file mode 100644
index 0000000000000000000000000000000000000000..cbcbb472458106123a806e8c67cae865344ef883
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.h
@@ -0,0 +1,97 @@
+/*
+ * 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
new file mode 100644
index 0000000000000000000000000000000000000000..df858aec05921a3bb2c749d6965529676a0265c7
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/CMakeLists.txt
@@ -0,0 +1,18 @@
+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 fa886cea9c7e4df76494bf05d3eb94101c8840c7..d48e381b98070a5aedf8218c066e60ff55cd8da3 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -17,6 +17,7 @@ add_subdirectory(ArViz)
 add_subdirectory(CartesianNaturalPositionController)
 add_subdirectory(ObjectPoseGui)
 add_subdirectory(CartesianImpedanceController)
+add_subdirectory(BimanualCartesianAdmittanceControllerGui)
 
 
 add_subdirectory(ArVizDrawerGui)
\ No newline at end of file
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index 03440c043481e7d38beefbb58853f7ac5a4c9e5a..0adf0413b30d5985851b86dab12580d0a314acbe 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -34,6 +34,7 @@ module data
     {
         const int NONE = 0;
         const int OVERRIDE_MATERIAL = 1;
+        const int HIDDEN = 2;
     };
 
     class Element
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 7cae7fe9e851dc8eaa152ea59cb8e1f8bd905adf..b4f2814c141564baadf2aabe762e9f01f98367e1 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -72,6 +72,7 @@ set(SLICE_FILES
 
     units/RobotUnit/NJointBimanualForceController.ice
     units/RobotUnit/NJointBimanualObjLevelController.ice
+    units/RobotUnit/NJointBimanualCartesianAdmittanceController.ice
     units/RobotUnit/NJointJointSpaceDMPController.ice
     units/RobotUnit/NJointTaskSpaceDMPController.ice
     units/RobotUnit/NJointBimanualForceMPController.ice
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.ice
new file mode 100644
index 0000000000000000000000000000000000000000..044d9f64fae26fc6a4f842ca2e82bea5eab8fca9
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.ice
@@ -0,0 +1,142 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::NJointControllerInterface
+ * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
+
+module armarx
+{
+    module detail
+    {
+        module NJBmanCartAdmCtrl
+        {
+            struct Box
+            {
+                //                Eigen::Matrix4f initialpose;
+                //                Eigen::Vector3f velXYZ;
+                //                Eigen::Vector3f velRPY;
+                float width;
+            };
+
+            struct Nullspace
+            {
+                Ice::FloatSeq desiredJointValuesLeft;
+                Ice::FloatSeq desiredJointValuesRight;
+                float k;
+                float d;
+            };
+            struct Impedance
+            {
+                Eigen::Vector3f KpXYZ;
+                Eigen::Vector3f KpRPY;
+                Eigen::Vector3f KdXYZ;
+                Eigen::Vector3f KdRPY;
+            };
+            struct Admittance
+            {
+                Eigen::Vector3f KpXYZ;
+                Eigen::Vector3f KpRPY;
+                Eigen::Vector3f KdXYZ;
+                Eigen::Vector3f KdRPY;
+                Eigen::Vector3f KmXYZ;
+                Eigen::Vector3f KmRPY;
+            };
+            struct Force
+            {
+                Eigen::Vector3f wrenchXYZ;
+                Eigen::Vector3f wrenchRPY;
+
+                // static compensation
+                float mass;
+                Eigen::Vector3f com;
+                Eigen::Vector3f offsetForce;
+                Eigen::Vector3f offsetTorque;
+
+                Eigen::Vector3f forceThreshold;
+            };
+        };
+    };
+
+    class NJointBimanualCartesianAdmittanceControllerConfig extends NJointControllerConfig
+    {
+        detail::NJBmanCartAdmCtrl::Box box;
+        detail::NJBmanCartAdmCtrl::Nullspace nullspace;
+        detail::NJBmanCartAdmCtrl::Impedance impedanceLeft;
+        detail::NJBmanCartAdmCtrl::Impedance impedanceRight;
+        detail::NJBmanCartAdmCtrl::Admittance admittanceObject;
+        detail::NJBmanCartAdmCtrl::Force forceLeft;
+        detail::NJBmanCartAdmCtrl::Force forceRight;
+
+        float filterCoeff;
+        float torqueLimit;
+        double ftCalibrationTime;
+
+        string kinematicChainRight;
+        string kinematicChainLeft;
+        string ftSensorRight;
+        string ftSensorLeft;
+        string ftSensorRightFrame;
+        string ftSensorLeftFrame;
+    };
+
+    interface NJointBimanualCartesianAdmittanceControllerInterface extends NJointControllerInterface
+    {
+        //        bool isAtGoal();
+        //        void runDMP(Ice::DoubleSeq goals, double timeDuration);
+        //        void runDMPWithVirtualStart(Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
+
+        //        //        void setViaPoints(double canVal, Ice::DoubleSeq point);
+        //        void setGoals(Ice::DoubleSeq goals);
+        //        void setViaPoints(double u, Ice::DoubleSeq viapoint);
+        void setConfig(NJointBimanualCartesianAdmittanceControllerConfig cfg);
+        void setDesiredJointValuesLeft(Ice::FloatSeq cfg);
+        void setDesiredJointValuesRight(Ice::FloatSeq cfg);
+        void setNullspaceConfig(detail::NJBmanCartAdmCtrl::Nullspace nullspace);
+        void setAdmittanceConfig(detail::NJBmanCartAdmCtrl::Admittance admittanceObject);
+        void setForceConfig(detail::NJBmanCartAdmCtrl::Force left, detail::NJBmanCartAdmCtrl::Force right);
+        void setImpedanceConfig(detail::NJBmanCartAdmCtrl::Impedance left, detail::NJBmanCartAdmCtrl::Impedance right);
+
+        ["cpp:const"] Eigen::Matrix4f getBoxPose();
+        void setBoxPose(Eigen::Matrix4f pose);
+        void setBoxWidth(float w);
+        void setBoxVelocity(Eigen::Vector3f velXYZ, Eigen::Vector3f velRPY);
+        void setBoxPoseAndVelocity(Eigen::Matrix4f pose,
+                                   Eigen::Vector3f velXYZ,
+                                   Eigen::Vector3f velRPY);
+        void moveBoxPose(Eigen::Matrix4f pose);
+        void moveBoxPosition(Eigen::Vector3f pos);
+
+        /*
+        Eigen::Matrix4f initialpose;
+        //                Eigen::Vector3f velXYZ;
+        //                Eigen::Vector3f velRPY;
+        */
+    };
+
+
+
+};
+
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h
index c57aff2d16196ce8e9425104addc9639000d548f..ee1bfca9a258eaba11052d5d0a997877fb6b8a3c 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h
@@ -26,7 +26,6 @@ namespace armarx
         const std::vector<ControlDevicePtr>& getCtrlDevs() const;
         const std::vector<SensorDevicePtr>& getSensDevs() const;
 
-    protected:
         void addSlave(const AbstractSlavePtr& slave);
         void addControlDevice(ControlDevicePtr dev);
         void addSensorDevice(SensorDevicePtr dev);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 80907d7b4a4fd5cdaf12d7c6372576987ee2df6e..3c56afba7a8bba669d95b3afd14438ebeaa00e9f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -18,10 +18,11 @@ namespace armarx
     {
     public:
         using path = std::filesystem::path;
+        inline static const std::string DefaultObjectsPackageName = "ArmarXObjects";
 
     public:
 
-        ObjectFinder(const std::string& objectsPackageName = "ArmarXObjects");
+        ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName);
 
         std::optional<ObjectInfo> findObject(const std::string& dataset, const std::string& name) const;
         std::optional<ObjectInfo> findObject(const std::string& nameOrID) const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
index 796c658992a1db36d884ff3f510fa994dc5bc80c..b756dc4aaac803245000b0deac6524ec7615a7bf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
@@ -25,10 +25,14 @@ namespace armarx
                     << ", but got: '" << nameOrID << "' (too many '/').";
             _dataset = split[0];
             _className = split[1];
+            if (split.size() == 3)
+            {
+                _instanceName = split[2];
+            }
         }
         else
         {
-            // dataset is left empty.
+            // Dataset, instanceName are left empty.
             _className = nameOrID;
         }
     }
@@ -43,6 +47,11 @@ namespace armarx
         return _str;
     }
 
+    bool ObjectID::equalClass(const ObjectID& rhs) const
+    {
+        return _className == rhs._className && _dataset == rhs._dataset;
+    }
+
     bool ObjectID::operator==(const ObjectID& rhs) const
     {
         return _className == rhs._className
@@ -52,14 +61,16 @@ namespace armarx
 
     bool ObjectID::operator<(const ObjectID& rhs) const
     {
-        if (_dataset != rhs._dataset)
+        int c = _dataset.compare(rhs._dataset);
+        if (c != 0)
         {
-            return _dataset < rhs._dataset;
+            return c < 0;
         }
         // equal dataset
-        if (_className != rhs._className)
+        c = _className.compare(rhs._className);
+        if (c != 0)
         {
-            return _className < rhs._className;
+            return c < 0;
         }
         // equal class name
         return _instanceName < rhs._instanceName;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
index 2b953ea4c2d126caaa0b41db612704e0649d284f..0d3bfb128d35ce5fa879c7abbbb24ff34404e7b7 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
@@ -14,7 +14,7 @@ namespace armarx
 
         ObjectID();
         ObjectID(const std::string& dataset, const std::string& className, const std::string& instancName = "");
-        /// Construct from either a name ("myobject") or ID ("mydataset/myobject", "mydataset/myclass/myinstance").
+        /// Construct from either a class name ("myobject") or ID ("mydataset/myobject", "mydataset/myclass/myinstance").
         ObjectID(const std::string& nameOrID);
 
 
@@ -38,7 +38,10 @@ namespace armarx
         /// Return "dataset/className" or "dataset/className/instanceName".
         std::string str() const;
 
+        /// Indicates whether dataset and class name are equal.
+        bool equalClass(const ObjectID& rhs) const;
 
+        /// Indicates whether dataset, class name and instance name are equal.
         bool operator==(const ObjectID& rhs) const;
         inline bool operator!=(const ObjectID& rhs) const
         {
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
index 2ad25f7ea27f28908cf50d0c950ba4170487fc85..4fe275d872221c3c49e83d9d295917e5d809c401 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
@@ -3,3 +3,4 @@
 SET(LIBS ${LIBS} ArmarXCore ${LIB_NAME})
 
 armarx_add_test(ArmarXObjectsTest ArmarXObjectsTest.cpp "${LIBS}")
+armarx_add_test(ArmarXObjects_ObjectIDTest ObjectIDTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d14a0d60406cd4074a6ab67d79885ad9041b5914
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
@@ -0,0 +1,121 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::ArmarXObjects
+ * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date       2020
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::ArmarXObjects
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../ArmarXObjects.h"
+
+#include "../ObjectID.h"
+
+#include <iostream>
+
+
+namespace armarx
+{
+    struct Fixture
+    {
+        std::vector<armarx::ObjectID> dcs
+        {
+            { "Data/Class/0" },
+            { "Data/Class/1" },
+            { "Data/Class/2" }
+        };
+        armarx::ObjectID dc0 { "Data/Class/0" };
+
+        std::vector<armarx::ObjectID> ots
+        {
+            { "Other/Type/0" },
+            { "Other/Type/1" },
+            { "Other/Type/2" }
+        };
+        armarx::ObjectID ot0 { "Other/Type/0" };
+    };
+}
+
+BOOST_FIXTURE_TEST_SUITE(ObjectIDTests, armarx::Fixture)
+
+BOOST_AUTO_TEST_CASE(test_construction_from_string)
+{
+    for (std::size_t i = 0; i < dcs.size(); ++i)
+    {
+        BOOST_CHECK_EQUAL(dcs[i].dataset(), "Data");
+        BOOST_CHECK_EQUAL(dcs[i].className(), "Class");
+        BOOST_CHECK_EQUAL(dcs[i].instanceName(), std::to_string(i));
+
+        BOOST_CHECK_EQUAL(ots[i].dataset(), "Other");
+        BOOST_CHECK_EQUAL(ots[i].className(), "Type");
+        BOOST_CHECK_EQUAL(ots[i].instanceName(), std::to_string(i));
+    }
+}
+
+BOOST_AUTO_TEST_CASE(test_equals_operator)
+{
+    for (std::size_t i = 0; i < dcs.size(); ++i)
+    {
+        BOOST_TEST_CONTEXT("i=" << i)
+        {
+            BOOST_CHECK_EQUAL(dcs[i], dcs[i]);
+            BOOST_CHECK_NE(dcs[i], ots[i]);
+            if (i != 0)
+            {
+                BOOST_CHECK_NE(dcs[i], dc0);
+            }
+        }
+    }
+}
+
+BOOST_AUTO_TEST_CASE(test_less_than_operator)
+{
+    for (std::size_t i = 0; i < dcs.size(); ++i)
+    {
+        for (std::size_t j = i; j < dcs.size(); ++j)
+        {
+            BOOST_CHECK_LE(dcs[i], dcs[j]);
+            BOOST_CHECK_GE(dcs[j], dcs[i]);
+            if (i != j)
+            {
+                BOOST_CHECK_LT(dcs[i], dcs[j]);
+                BOOST_CHECK_GT(dcs[j], dcs[i]);
+            }
+        }
+    }
+}
+
+BOOST_AUTO_TEST_CASE(test_equalClass)
+{
+    for (std::size_t i = 0; i < dcs.size(); ++i)
+    {
+        for (std::size_t j = 0; j < dcs.size(); ++j)
+        {
+            BOOST_CHECK(dcs[i].equalClass(dcs[j]));
+            BOOST_CHECK(ots[i].equalClass(ots[j]));
+
+            BOOST_CHECK(!dcs[i].equalClass(ots[j]));
+            BOOST_CHECK(!ots[i].equalClass(dcs[j]));
+        }
+    }
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2e61a616541c4fad51225d6f363dd9dfa4d20a7a
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp
@@ -0,0 +1,884 @@
+#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
new file mode 100644
index 0000000000000000000000000000000000000000..3a10a4bb32861310989f309391485f389d7b083f
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h
@@ -0,0 +1,278 @@
+#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/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index b9d558a2e6d4fdc6b6367d3a897f6455244100e5..4e1caa7a466841f031892e99276e5e22070cfa59 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -39,6 +39,7 @@ list(APPEND LIB_HEADERS
    BimanualForceControllers/NJointBimanualObjLevelController.h
    BimanualForceControllers/NJointBimanualObjLevelVelController.h
    BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
+   BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h
 
    )
 list(APPEND LIB_FILES
@@ -59,6 +60,7 @@ list(APPEND LIB_FILES
    BimanualForceControllers/NJointBimanualObjLevelController.cpp
    BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
    BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
+   BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp
    )
 
 list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index 1021205caf6ade46e5d5330df8f5591591b01430..2846da675f26f263aa244715205b490d78d4803b 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -43,7 +43,7 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
 
         void update(const RobotUnitDataStreaming::TimeStepSeq& data, const Ice::Current&) override
         {
-            std::lock_guard{_data_mutex};
+            std::lock_guard g{_data_mutex};
             ARMARX_INFO << deactivateSpam()
                         << "received " << data.size() << " timesteps";
             _data.emplace_back(data);
@@ -98,7 +98,7 @@ namespace armarx
         ARMARX_CHECK_NOT_NULL(_receiver);
         std::deque<RobotUnitDataStreaming::TimeStepSeq> data;
         {
-            std::lock_guard{_receiver->_data_mutex};
+            std::lock_guard g{_receiver->_data_mutex};
             std::swap(data, _receiver->_data);
         }
         for (auto& chunk : data)