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