modify ts adaptive controller

source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice

@@ -358,7 +358,6 @@ module armarx
         float torqueLimit;
         string forceSensorName;
         float filterCoeff;
-        bool isAdaptiveControl;
     };
 
     interface NJointTaskSpaceAdaptiveDMPControllerInterface extends NJointControllerInterface
@@ -374,6 +373,7 @@ module armarx
         void setGoals(Ice::DoubleSeq goals);
 
         double getVirtualTime();
+        void setCanVal(double canVal);
 
         void resetDMP();
         void stopDMP();

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp

@@ -143,6 +143,11 @@ namespace armarx
         initInt2rtData.KdImpedance = KdImpedance;
         interface2rtBuffer.reinitAllBuffers(initInt2rtData);
 
+
+        Interface2CtrlData initInt2CtrlData;
+        initInt2CtrlData.canVal = 1.0;
+        interface2CtrlBuffer.reinitAllBuffers(initInt2CtrlData);
+
         firstRun = true;
         forceOffset.setZero(3);
         filteredForce.setZero(3);
@@ -189,6 +194,11 @@ namespace armarx
         Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
         Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
 
+        if (interface2CtrlBuffer.updateReadBuffer())
+        {
+            dmpCtrl->canVal = interface2CtrlBuffer.getUpToDateReadBuffer().canVal;
+        }
+
         if (!started)
         {
             LockGuardType guard {controlDataMutex};
@@ -309,13 +319,10 @@ namespace armarx
         interfaceData.commitWrite();
 
 
-        if (cfg->isAdaptiveControl)
-        {
-            kpos = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.head(3);
-            kori = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.tail(3);
-            dpos = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.head(3);
-            dori = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.tail(3);
-        }
+        kpos = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.head(3);
+        kori = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.tail(3);
+        dpos = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.head(3);
+        dori = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.tail(3);
 
         /* calculate torques in meter */
         jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
@@ -378,6 +385,17 @@ namespace armarx
         debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
         debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
 
+        debugOutputData.getWriteBuffer().forceInRoot_x = filteredForce(0);
+        debugOutputData.getWriteBuffer().forceInRoot_y = filteredForce(1);
+        debugOutputData.getWriteBuffer().forceInRoot_z = filteredForce(2);
+        //        debugOutputData.getWriteBuffer().torqueInRoot_x = filteredForce(3);
+        //        debugOutputData.getWriteBuffer().torqueInRoot_y = filteredForce(4);
+        //        debugOutputData.getWriteBuffer().torqueInRoot_z = filteredForce(5);
+
+        debugOutputData.getWriteBuffer().vel_x = currentTwist(0);
+        debugOutputData.getWriteBuffer().vel_y = currentTwist(1);
+        debugOutputData.getWriteBuffer().vel_z = currentTwist(2);
+
         //        debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
         //        debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
 
@@ -425,6 +443,14 @@ namespace armarx
 
     }
 
+    void NJointTaskSpaceAdaptiveDMPController::setCanVal(double canVal, const Ice::Current&)
+    {
+        LockGuardType guard(int2ctrlMutex);
+        interface2CtrlBuffer.getWriteBuffer().canVal = canVal;
+        interface2CtrlBuffer.commitWrite();
+    }
+
+
     void NJointTaskSpaceAdaptiveDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&)
     {
         DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
@@ -618,6 +644,14 @@ namespace armarx
         datafields["forceDesired_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
         datafields["forceDesired_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
 
+        datafields["forceInRoot_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_x);
+        datafields["forceInRoot_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_y);
+        datafields["forceInRoot_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_z);
+
+        datafields["vel_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_x);
+        datafields["vel_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_y);
+        datafields["vel_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_z);
+
         datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
 
         std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h

@@ -76,6 +76,7 @@ namespace armarx
         void setKpImpedance(const Ice::FloatSeq& stiffness, const Ice::Current&);
         Ice::FloatSeq getForce(const Ice::Current&);
         Ice::FloatSeq getVelocityInMM(const Ice::Current&);
+        void setCanVal(double canVal, const Ice::Current&);
 
     protected:
         virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
@@ -114,6 +115,17 @@ namespace armarx
             float forceDesired_ry;
             float forceDesired_rz;
 
+            float forceInRoot_x;
+            float forceInRoot_y;
+            float forceInRoot_z;
+            //            float torqueInRoot_x;
+            //            float torqueInRoot_y;
+            //            float torqueInRoot_z;
+
+            float vel_x;
+            float vel_y;
+            float vel_z;
+
             float deltaT;
 
         };
@@ -147,6 +159,12 @@ namespace armarx
         };
         TripleBuffer<Inferface2rtData> interface2rtBuffer;
 
+        struct Interface2CtrlData
+        {
+            double canVal;
+        };
+        TripleBuffer<Interface2CtrlData> interface2CtrlBuffer;
+
 
         DMP::Vec<DMP::DMPState> currentJointState;
         DMP::UMIDMPPtr nullSpaceJointDMPPtr;
@@ -204,6 +222,7 @@ namespace armarx
         Eigen::Vector3f filteredForce;
 
         mutable MutexType interfaceDataMutex;
+        mutable MutexType int2ctrlMutex;
 
         // NJointController interface
     protected: