From 49ebf77ff94f6a5e40a596ca282d467abeca1ac3 Mon Sep 17 00:00:00 2001
From: zhou <you.zhou@kit.edu>
Date: Thu, 22 Jul 2021 09:38:31 +0200
Subject: [PATCH] working on force intergration to impedence controller
---
.../NJointTaskSpaceDMPController.ice | 7 ++
.../NJointTaskSpaceImpedanceDMPController.cpp | 64 ++++++++-----------
.../NJointTaskSpaceImpedanceDMPController.h | 21 +++++-
3 files changed, 53 insertions(+), 39 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 93b018801..878f0b28b 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -298,6 +298,7 @@ module armarx
string dmpType = "Discrete";
double timeDuration;
string nodeSetName;
+ string forceSensorName;
// phaseStop technique
double phaseL = 10;
@@ -306,6 +307,7 @@ module armarx
double phaseDist1 = 10;
double posToOriRatio = 100;
+
Ice::FloatSeq Kpos;
Ice::FloatSeq Dpos;
Ice::FloatSeq Kori;
@@ -319,6 +321,8 @@ module armarx
float torqueLimit;
+ float waitTimeForCalibration;
+
};
interface NJointTaskSpaceImpedanceDMPControllerInterface extends NJointControllerInterface
@@ -348,6 +352,9 @@ module armarx
void setAngularVelocityKd(Ice::FloatSeq kd);
void setAngularVelocityKp(Ice::FloatSeq kp);
+ void enableForceStop();
+ void disableForceStop();
+ void setForceThreshold(float forceThreshold);
};
class NJointTaskSpaceAdaptiveDMPControllerConfig extends NJointControllerConfig
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 65da4d862..945b5d695 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -37,6 +37,13 @@ namespace armarx
positionSensors.push_back(positionSensor);
};
+ const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
+ forceSensor = svlf->asA<SensorValueForceTorque>();
+
+ forceOffset.setZero();
+ filteredForce.setZero();
+ filteredForceInRoot.setZero();
+
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
numOfJoints = targets.size();
@@ -135,8 +142,6 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::controllerRun()
{
-
-
if (!dmpCtrl)
{
return;
@@ -226,6 +231,22 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
double deltaT = timeSinceLastIteration.toSecondsDouble();
+ Eigen::Matrix4f targetPose;
+ Eigen::VectorXf targetVel;
+ Eigen::VectorXf desiredNullSpaceJointValues;
+ if (firstRun || !started)
+ {
+ firstRun = false;
+ targetPose = currentPose;
+ targetVel.setZero(6);
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ }
+ else
+ {
+ targetPose = rtGetControlStruct().targetPose;
+ targetVel = rtGetControlStruct().targetVel;
+ desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
+ }
Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
@@ -253,22 +274,6 @@ namespace armarx
jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
- Eigen::Matrix4f targetPose;
- Eigen::VectorXf targetVel;
- Eigen::VectorXf desiredNullSpaceJointValues;
- if (firstRun)
- {
- firstRun = false;
- targetPose = currentPose;
- targetVel.setZero(6);
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
- }
- else
- {
- targetPose = rtGetControlStruct().targetPose;
- targetVel = rtGetControlStruct().targetVel;
- desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
- }
Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
@@ -442,34 +447,17 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
{
- firstRun = true;
- while (firstRun)
- {
- usleep(100);
- }
-
- Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
dmpCtrl->canVal = timeDuration;
dmpCtrl->config.motionTimeDuration = timeDuration;
- finished = false;
-
- if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
- {
- ARMARX_INFO << "Using Null Space Joint DMP";
- }
-
- started = true;
- stopped = false;
- // controllerTask->start();
+ runDMP(goals);
}
void NJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
{
firstRun = true;
- while (firstRun)
+ timeForCalibration = 0;
+ while (firstRun || timeForCalibration < cfg.waitTimeForCalibration)
{
usleep(100);
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 1b49c9963..d68faf549 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -81,7 +81,18 @@ namespace armarx
void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)override;
void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)override;
-
+ void enableForceStop(const Ice::Current&) override
+ {
+ useForceStop = true;
+ }
+ void disableForceStop(const Ice::Current&) override
+ {
+ useForceStop = false;
+ }
+ void setForceThreshold(float forceThreshold, const Ice::Current&) override
+ {
+ this->forceThreshold = forceThreshold;
+ }
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
@@ -152,6 +163,7 @@ namespace armarx
TripleBuffer<CtrlParams> ctrlParams;
+
DMP::Vec<DMP::DMPState> currentJointState;
DMP::UMIDMPPtr nullSpaceJointDMPPtr;
@@ -202,6 +214,13 @@ namespace armarx
bool started = false;
bool stopped = false;
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f forceOffset;
+ Eigen::Vector3f filteredForceInRoot;
+ std::atomic<float> forceThreshold;
+ std::atomic<bool> useForceStop;
+ std::atomic<float> timeForCalibration;
+
Eigen::Matrix4f oldPose;
// NJointController interface
protected:
--
GitLab