diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index c8b8d1af2a9af55f243ae91bff7110eec0d63928..4b0bdae564a7cc28e22c5cd028f3846c6753037c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -398,6 +398,10 @@ namespace armarx
             ARMARX_DEBUG << "ForceTorqueUnit initialized";
             initializeInertialMeasurementUnit();
             ARMARX_DEBUG << "InertialMeasurementUnit initialized";
+            initializeTrajectoryControllerUnit();
+            ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
+            initializeTcpControllerUnit();
+            ARMARX_DEBUG << "TcpControllerUnit initialized";
         }
         ARMARX_INFO << "initializing default units...done! " << (MicroNow() - beg).count() << " us";
     }
@@ -607,6 +611,35 @@ namespace armarx
         addUnit(std::move(unit));
     }
 
+    void RobotUnit::initializeTrajectoryControllerUnit()
+    {
+        auto guard = getGuard();
+        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+        using UnitT = TrajectoryControllerSubUnit;
+
+        //check if unit is already added
+        if (getUnit(UnitT::ice_staticId()))
+        {
+            return;
+        }
+        (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))->setup(this);
+        addUnit(trajectoryControllerSubUnit);
+    }
+
+    void RobotUnit::initializeTcpControllerUnit()
+    {
+        auto guard = getGuard();
+        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+        using UnitT = TCPControllerSubUnit;
+
+        //check if unit is already added
+        if (getUnit(UnitT::ice_staticId()))
+        {
+            return;
+        }
+        (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))->setup(this, robot->clone());
+        addUnit(tcpControllerSubUnit);
+    }
     void armarx::RobotUnit::initializeInertialMeasurementUnit()
     {
         using UnitT = InertialMeasurementSubUnit;
@@ -2954,10 +2987,6 @@ namespace armarx
         debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue());
         debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverTopicName").getValue());
         onConnectRobotUnit();
-        (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))->setup(this, robot->clone());
-        getArmarXManager()->addObjectAsync(tcpControllerSubUnit, "", true, false);
-        (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))->setup(this);
-        getArmarXManager()->addObjectAsync(trajectoryControllerSubUnit, "", true, false);
         startSelfCollisionAvoidance();
         ARMARX_DEBUG << "RobotUnit::onConnectComponent()...done!";
     }
@@ -2968,8 +2997,6 @@ namespace armarx
         stopSelfCollisionAvoidance();
 
         // Disconnecting TCPController and TrajectoryController
-        getArmarXManager()->removeObjectBlocking(tcpControllerSubUnit->getName());
-        getArmarXManager()->removeObjectBlocking(trajectoryControllerSubUnit->getName());
 
         onDisconnectRobotUnit();
         ARMARX_DEBUG << "RobotUnit::onDisconnectComponent()...done!";
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 7674e93d90a4771271e98174fc9219faab8bbbb2..448e444ac493a5950cb409a6f6a4bc11993b15b6 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -679,6 +679,8 @@ namespace armarx
         virtual void initializePlatformUnit();
         virtual void initializeForceTorqueUnit();
         virtual void initializeInertialMeasurementUnit();
+        virtual void initializeTrajectoryControllerUnit();
+        virtual void initializeTcpControllerUnit();
         void addUnit(ManagedIceObjectPtr unit);
         void finishUnitInitialization();
         // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //