fixed tcp control subunit

source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp

@@ -70,7 +70,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
         ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNode(tcpName), "The robot does not have the node: " + tcpName);
         tcp = tcpName;
     }
-
+    auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName;
     auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
     NJointCartesianVelocityControllerWithRampPtr tcpController;
     bool nodeSetAlreadyControlled = false;
@@ -81,7 +81,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
         {
             continue;
         }
-        if (tcpController->getNodeSetName() == nodeSetName)
+        if (tcpController->getNodeSetName() == nodeSetName && tcpController->getInstanceName() == controllerName)
         {
             nodeSetAlreadyControlled = true;
             break;
@@ -136,10 +136,10 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
     }
     ARMARX_DEBUG << "CartesianSelection-Mode: " << (int)mode;
 
-    auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName;
+
     if (!nodeSetAlreadyControlled)
     {
-        NJointCartesianVelocityControllerWithRampConfigPtr config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, tcp, mode, 500, 1, 2, 1, 2);
+        NJointCartesianVelocityControllerWithRampConfigPtr config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, tcp, mode, 500, 1, 2, 0.3f, 2);
         //        NJointCartesianVelocityControllerConfigPtr config = new NJointCartesianVelocityControllerConfig(nodeSetName, tcp, mode);
         NJointCartesianVelocityControllerWithRampPtr ctrl = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(
                     robotUnit->createNJointController(
@@ -168,7 +168,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
         //        }
         if (!tcpController->isControllerActive())
         {
-            robotUnit->activateNJointController(controllerName);
+            tcpController->activateController();
         }
     }
 }