From d20e71557cfb1fe502a69602755bdc7e48eddd1d Mon Sep 17 00:00:00 2001
From: zhou <you.zhou@kit.edu>
Date: Tue, 1 Jun 2021 10:36:03 +0200
Subject: [PATCH] added control parameters function in
 NJointTaskSpaceImpedanceCtrl

---
 .../NJointTaskSpaceDMPController.ice          |  7 ++-
 .../NJointTaskSpaceImpedanceDMPController.cpp | 52 +++++++++++++++++--
 .../NJointTaskSpaceImpedanceDMPController.h   | 25 +++++++--
 3 files changed, 74 insertions(+), 10 deletions(-)

diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 38cdb96be..93b018801 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -314,8 +314,6 @@ module armarx
         Ice::FloatSeq Knull;
         Ice::FloatSeq Dnull;
 
-
-
         bool useNullSpaceJointDMP;
         Ice::FloatSeq defaultNullSpaceJointValues;
 
@@ -345,6 +343,11 @@ module armarx
         void setMPWeights(DoubleSeqSeq weights);
         DoubleSeqSeq getMPWeights();
 
+        void setLinearVelocityKd(Ice::FloatSeq kd);
+        void setLinearVelocityKp(Ice::FloatSeq kp);
+        void setAngularVelocityKd(Ice::FloatSeq kd);
+        void setAngularVelocityKp(Ice::FloatSeq kp);
+
     };
 
     class NJointTaskSpaceAdaptiveDMPControllerConfig extends NJointControllerConfig
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 70fae3d0b..65da4d862 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -74,11 +74,13 @@ namespace armarx
         }
 
 
-        kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
-        dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
-        kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
-        dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+        Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
+        Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
+        Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
+        Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
 
+        CtrlParams initParams = {kpos, dpos, kori, dori};
+        ctrlParams.reinitAllBuffers(initParams);
 
         ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
         ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
@@ -267,6 +269,12 @@ namespace armarx
             targetVel = rtGetControlStruct().targetVel;
             desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
         }
+
+        Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
+        Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
+        Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
+        Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
+
         Eigen::Vector6f jointControlWrench;
         {
             Eigen::Vector3f targetTCPLinearVelocity;
@@ -582,5 +590,41 @@ namespace armarx
         dmpCtrl->removeAllViaPoints();
     }
 
+    void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+    {
+        ARMARX_CHECK_EQUAL(kd.size(), 3);
+        LockGuardType guard(controllerMutex);
+        ctrlParams.getWriteBuffer().dpos << kd[0], kd[1], kd[2];
+        ctrlParams.commitWrite();
+
+    }
+
+    void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+    {
+        ARMARX_CHECK_EQUAL(kp.size(), 3);
+        LockGuardType guard(controllerMutex);
+        ctrlParams.getWriteBuffer().kpos << kp[0], kp[1], kp[2];
+        ctrlParams.commitWrite();
+    }
+
+    void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+    {
+        ARMARX_CHECK_EQUAL(kd.size(), 3);
+        LockGuardType guard(controllerMutex);
+        ctrlParams.getWriteBuffer().dori << kd[0], kd[1], kd[2];
+        ctrlParams.commitWrite();
+
+    }
+
+    void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+    {
+        ARMARX_CHECK_EQUAL(kp.size(), 3);
+        LockGuardType guard(controllerMutex);
+        ctrlParams.getWriteBuffer().kori << kp[0], kp[1], kp[2];
+        ctrlParams.commitWrite();
+
+
+    }
+
 
 }
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 34bd9785c..1b49c9963 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -76,6 +76,12 @@ namespace armarx
 
         void removeAllViaPoints(const Ice::Current&) override;
 
+        void setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
+        void setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)override;
+        void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)override;
+        void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)override;
+
+
     protected:
         virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
@@ -135,6 +141,17 @@ namespace armarx
 
         TripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
 
+        struct CtrlParams
+        {
+            Eigen::Vector3f kpos;
+            Eigen::Vector3f dpos;
+            Eigen::Vector3f kori;
+            Eigen::Vector3f dori;
+        };
+
+        TripleBuffer<CtrlParams> ctrlParams;
+
+
         DMP::Vec<DMP::DMPState> currentJointState;
         DMP::UMIDMPPtr nullSpaceJointDMPPtr;
 
@@ -165,10 +182,10 @@ namespace armarx
 
         float torqueLimit;
 
-        Eigen::Vector3f kpos;
-        Eigen::Vector3f kori;
-        Eigen::Vector3f dpos;
-        Eigen::Vector3f dori;
+        //        Eigen::Vector3f kpos;
+        //        Eigen::Vector3f kori;
+        //        Eigen::Vector3f dpos;
+        //        Eigen::Vector3f dori;
         Eigen::VectorXf knull;
         Eigen::VectorXf dnull;
         int numOfJoints;
-- 
GitLab