TCPControllerSubUnit: Use NJointCartesianVelocityControllerWithRamp controler

TCPControllerSubUnit: Fixed isRequested()

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp

@@ -176,6 +176,11 @@ namespace armarx
 
     }
 
+    const std::string& NJointCartesianVelocityControllerWithRamp::getNodeSetName() const
+    {
+        return nodeSetName;
+    }
+
     WidgetDescription::StringWidgetDictionary NJointCartesianVelocityControllerWithRamp::getFunctionDescriptions(const Ice::Current&) const
     {
         return

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h

@@ -143,6 +143,7 @@ namespace armarx
         void setJointLimitAvoidanceScale(float jointLimitAvoidanceScale, const Ice::Current&) override;
         void setKpJointLimitAvoidance(float KpJointLimitAvoidance, const Ice::Current&) override;
         void immediateHardStop(const Ice::Current&) override;
+        const std::string& getNodeSetName() const;
     };
 
 } // namespace armarx

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

@@ -23,8 +23,10 @@
 #include "TCPControllerSubUnit.h"
 
 #include <RobotAPI/libraries/core/FramedPose.h>
-
 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+
 
 using namespace armarx;
 
@@ -51,11 +53,12 @@ void TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
 
 void TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
 {
-    ARMARX_WARNING << "Setting cycle time in RT-Controller does not have an effect";
+    ARMARX_WARNING << deactivateSpam() << "Setting cycle time in RT-Controller does not have an effect";
 }
 
 void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const FramedDirectionBasePtr& translationVelocity, const FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c)
 {
+    ScopedLock lock(dataMutex);
     ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
     std::string tcp;
     if (tcpName.empty())
@@ -69,11 +72,11 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
     }
 
     auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
-    NJointCartesianVelocityControllerPtr tcpController;
+    NJointCartesianVelocityControllerWithRampPtr tcpController;
     bool nodeSetAlreadyControlled = false;
     for (NJointControllerPtr controller : activeNJointControllers)
     {
-        tcpController = NJointCartesianVelocityControllerPtr::dynamicCast(controller);
+        tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
         if (!tcpController)
         {
             continue;
@@ -113,19 +116,19 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
 
     }
 
-    NJointCartesianVelocityControllerMode::CartesianSelection mode = NJointCartesianVelocityControllerMode::eAll;
+    CartesianSelectionMode::CartesianSelection mode = CartesianSelectionMode::eAll;
     bool noMode = false;
     if (globalTransVel && globalOriVel)
     {
-        mode = NJointCartesianVelocityControllerMode::eAll;
+        mode = CartesianSelectionMode::eAll;
     }
     else if (globalTransVel && !globalOriVel)
     {
-        mode = NJointCartesianVelocityControllerMode::ePosition;
+        mode = CartesianSelectionMode::ePosition;
     }
     else if (!globalTransVel && globalOriVel)
     {
-        mode = NJointCartesianVelocityControllerMode::eOrientation;
+        mode = CartesianSelectionMode::eOrientation;
     }
     else
     {
@@ -136,16 +139,18 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
     auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName;
     if (!nodeSetAlreadyControlled)
     {
-        NJointCartesianVelocityControllerConfigPtr config = new NJointCartesianVelocityControllerConfig(nodeSetName, tcp, mode);
-        NJointCartesianVelocityControllerPtr ctrl = NJointCartesianVelocityControllerPtr::dynamicCast(
+        NJointCartesianVelocityControllerWithRampConfigPtr config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, tcp, mode, 500, 1, 2, 1, 2);
+        //        NJointCartesianVelocityControllerConfigPtr config = new NJointCartesianVelocityControllerConfig(nodeSetName, tcp, mode);
+        NJointCartesianVelocityControllerWithRampPtr ctrl = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(
                     robotUnit->createNJointController(
-                        "NJointCartesianVelocityController", controllerName,
+                        "NJointCartesianVelocityControllerWithRamp", controllerName,
                         config, true, true
                     )
                 );
 
         tcpController = ctrl;
-        tcpController->setAvoidJointLimitsKp(getProperty<float>("AvoidJointLimitsKp").getValue());
+        tcpControllerNameMap[nodeSetName] = controllerName;
+        tcpController->setKpJointLimitAvoidance(getProperty<float>("AvoidJointLimitsKp").getValue(), c);
     }
 
 
@@ -155,7 +160,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
     {
         ARMARX_CHECK_EXPRESSION(tcpController);
         //        ARMARX_CHECK_EXPRESSION(tcpController->getObjectScheduler());
-        tcpController->setVelocities(xVel, yVel, zVel, rollVel, pitchVel, yawVel, NJointCartesianVelocityController::ModeFromIce(mode));
+        tcpController->setTargetVelocity(xVel, yVel, zVel, rollVel, pitchVel, yawVel, c);
         //        if (!tcpController->getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted, 5000))
         //        {
         //            ARMARX_ERROR << "NJointController was not initialized after 5000ms - bailing out!";
@@ -170,16 +175,15 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
 
 bool TCPControllerSubUnit::isRequested(const Ice::Current&)
 {
-    auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
-    for (NJointControllerPtr controller : activeNJointControllers)
+    ScopedLock lock(dataMutex);
+    for (auto& pair : tcpControllerNameMap)
     {
-        auto tcpController = NJointCartesianVelocityControllerPtr::dynamicCast(controller);
-        if (!tcpController)
+        auto ctrl = robotUnit->getNJointController(pair.second);
+        if (ctrl && ctrl->isControllerActive())
         {
             return true;
         }
     }
-
     return false;
 }
 
@@ -191,10 +195,10 @@ void armarx::TCPControllerSubUnit::componentPropertiesUpdated(const std::set<std
         auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
         for (NJointControllerPtr controller : activeNJointControllers)
         {
-            auto tcpController = NJointCartesianVelocityControllerPtr::dynamicCast(controller);
+            auto tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
             if (tcpController)
             {
-                tcpController->setAvoidJointLimitsKp(avoidJointLimitsKp);
+                tcpController->setKpJointLimitAvoidance(avoidJointLimitsKp, GlobalIceCurrent);
             }
         }
     }

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

@@ -27,7 +27,6 @@
 #include "RobotUnitSubUnit.h"
 #include "../NJointControllers/NJointTCPController.h"
 #include "../util.h"
-
 namespace armarx
 {
     TYPEDEF_PTRS_HANDLE(RobotUnit);
@@ -58,9 +57,10 @@ namespace armarx
         // RobotUnitSubUnit interface
         void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
     private:
-        //        mutable std::mutex dataMutex;
+        mutable Mutex dataMutex;
         RobotUnit* robotUnit = nullptr;
         VirtualRobot::RobotPtr coordinateTransformationRobot;
+        std::map<std::string, std::string> tcpControllerNameMap;
 
         // ManagedIceObject interface
     protected: