added setTarget function for taskspaceimpedence

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

@@ -232,5 +232,19 @@ void NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&, cons
 
 }
 
+void NJointTaskSpaceImpedanceController::setTarget(const Ice::FloatSeq& target, const Ice::Current&)
+{
+    Eigen::VectorXf desiredPose;
+    desiredPose.resize(7);
+    for (size_t i = 0; i < 7; ++i)
+    {
+        desiredPose(i) = target.at(i);
+    }
+
+    LockGuardType guard {controlDataMutex};
+    getWriterControlStruct().desiredPose = desiredPose;
+    writeControlStruct();
+}
+
 
 

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

@@ -70,7 +70,7 @@ namespace armarx
     protected:
         void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
-
+        void setTarget(const Ice::FloatSeq&, const Ice::Current&);
     private:
         Eigen::Vector3f desiredPosition;
         Eigen::Quaternionf desiredQuaternion;

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

@@ -47,7 +47,7 @@ module armarx
 
     interface NJointTaskSpaceImpedanceControlInterface extends NJointControllerInterface
     {
-
+        void setTarget(Ice::FloatSeq target);
     };
 
 };

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

@@ -255,12 +255,11 @@ namespace armarx
 
     void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
     {
-        LockGuardType guard(controllerMutex);
+        //        LockGuardType guard(controllerMutex);
         ARMARX_INFO << "setting via points ";
         dmpCtrl->setViaPose(u, viapoint);
     }
 
-
     void NJointTaskSpaceImpedanceDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
     {
         dmpCtrl->setGoalPoseVec(goals);
@@ -348,7 +347,9 @@ namespace armarx
         datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
         datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
 
-        debugObs->setDebugChannel("TaskSpaceImpedanceDMP", datafields);
+
+        std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
+        debugObs->setDebugChannel(channelName, datafields);
     }
 
     void NJointTaskSpaceImpedanceDMPController::onInitComponent()