From ece7c9fcb9e25e6f6eb7c8b003ed10aa2ef8c933 Mon Sep 17 00:00:00 2001
From: Armar6 <armar6@h2t.com>
Date: Sun, 3 Mar 2019 13:17:35 +0100
Subject: [PATCH] added adaptive position control
---
.../NJointTaskSpaceDMPController.ice | 3 +
.../DMPController/TaskSpaceDMPController.cpp | 2 +-
...NJointPeriodicTSDMPCompliantController.cpp | 119 ++++++++++++------
.../NJointPeriodicTSDMPCompliantController.h | 6 +
4 files changed, 93 insertions(+), 37 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 3c303ee6d..d2ba295d3 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -361,6 +361,9 @@ module armarx
Ice::FloatSeq ws_y;
Ice::FloatSeq ws_z;
+ float adaptCoeff;
+ float reactThreshold;
+
};
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
index e221ee7d4..ed5f720b5 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -81,7 +81,7 @@ double TaskSpaceDMPController::flow(double canVal, double deltaT, const Eigen::M
}
double timeDuration = config.motionTimeDuration;
- canVal -= tau * deltaT * 1 / (1 + phaseStop) ;
+ canVal -= tau * deltaT * 1;// / (1 + phaseStop) ;
DMP::Vec<DMP::DMPState > temporalState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index d4ad2769f..3be08e9d7 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -91,6 +91,9 @@ namespace armarx
forceOffset.setZero();
filteredForce.setZero();
+ reactForce.setZero();
+ filteredForceInRoot.setZero();
+
UserToRTData initUserData;
initUserData.targetForce = 0;
@@ -175,13 +178,13 @@ namespace armarx
return;
}
- if (!rt2CtrlData.updateReadBuffer() || !dmpCtrl)
+ if (!dmpCtrl)
{
return;
}
Eigen::VectorXf targetVels(6);
- bool isPhaseStop = rt2CtrlData.getReadBuffer().isPhaseStop;
+ bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
if (isPhaseStop)
{
targetVels.setZero();
@@ -189,9 +192,9 @@ namespace armarx
else
{
- double deltaT = rt2CtrlData.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
+ double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
LockGuardType guard {controllerMutex};
dmpCtrl->flow(deltaT, currentPose, currentTwist);
@@ -291,12 +294,12 @@ namespace armarx
}
}
Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
- Eigen::Vector3f filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
+ filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri;
- float desiredZVel = kpf * (targetForce - filteredForceInRoot.norm());
+ float desiredZVel = kpf * (targetForce - filteredForceInRoot(2));
targetVel(2) -= desiredZVel;
// targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
@@ -347,23 +350,21 @@ namespace armarx
}
bool isPhaseStop = false;
-
-
+ Eigen::Vector3f adaptK = kpos;
float diff = (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)).norm();
if (diff > cfg->phaseDist0)
{
isPhaseStop = true;
+ adaptK = ((float)exp(-fabs(diff - cfg->phaseDist0) * cfg->adaptCoeff)) * adaptK;
+ adaptK(2) = kpos(2);
}
- rt2CtrlData.getWriteBuffer().currentPose = currentPose;
- rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
- rt2CtrlData.getWriteBuffer().deltaT = deltaT;
- rt2CtrlData.getWriteBuffer().currentTime += deltaT;
- rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
- rt2CtrlData.commitWrite();
+ // adaptive control with distance
+
+
float dTf = (float)deltaT;
@@ -378,14 +379,46 @@ namespace armarx
targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
debugRT.getWriteBuffer().targetPose = targetPose;
+ debugRT.getWriteBuffer().filteredForce = filteredForceInRoot;
+ debugRT.getWriteBuffer().reactForce = reactForce;
+ debugRT.getWriteBuffer().targetVel = targetVel;
+ debugRT.getWriteBuffer().adaptK = adaptK;
+
debugRT.commitWrite();
+ if (isPhaseStop)
+ {
+ reactForce += filteredForceInRoot * dTf;
+ float xyforce = reactForce.block<2, 1>(0, 0).norm();
+ if (xyforce > cfg->reactThreshold)
+ {
+ targetPose(0, 3) = currentPose(0, 3);
+ targetPose(1, 3) = currentPose(1, 3);
+ isPhaseStop = false;
+ reactForce << 0, 0, 0;
+ }
+ }
+ else
+ {
+ reactForce << 0, 0, 0;
+ }
+
+ rt2CtrlData.getWriteBuffer().currentPose = currentPose;
+ rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
+ rt2CtrlData.getWriteBuffer().deltaT = deltaT;
+ rt2CtrlData.getWriteBuffer().currentTime += deltaT;
+ rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
+ rt2CtrlData.commitWrite();
+
// Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
// targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
// inverse dynamic controller
jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
+
+
+
Eigen::Vector6f jointControlWrench;
{
Eigen::Vector3f targetTCPLinearVelocity;
@@ -395,19 +428,19 @@ namespace armarx
Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f linearVel ;
- if (isPhaseStop)
- {
- linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
- for (size_t i = 0; i < 3; ++i)
- {
- linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
- }
- }
- else
- {
- linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
- }
+ Eigen::Vector3f linearVel = adaptK.cwiseProduct(desiredPosition - currentTCPPosition);;
+ // if (isPhaseStop)
+ // {
+ // linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
+ // for (size_t i = 0; i < 3; ++i)
+ // {
+ // linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
+ // }
+ // }
+ // else
+ // {
+ // linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
+ // }
Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
Eigen::Vector3f currentTCPAngularVelocity;
@@ -527,15 +560,29 @@ namespace armarx
datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
- // Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
- // datafields["filtered_force_x"] = new Variant(filteredForce(0));
- // datafields["filtered_force_y"] = new Variant(filteredForce(1));
- // datafields["filtered_force_z"] = new Variant(filteredForce(2));
+ Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
+ datafields["filteredforce_x"] = new Variant(filteredForce(0));
+ datafields["filteredforce_y"] = new Variant(filteredForce(1));
+ datafields["filteredforce_z"] = new Variant(filteredForce(2));
+
+
+ Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
+ datafields["reactForce_x"] = new Variant(reactForce(0));
+ datafields["reactForce_y"] = new Variant(reactForce(1));
+ datafields["reactForce_z"] = new Variant(reactForce(2));
+
+ Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
+ datafields["targetVel_x"] = new Variant(targetVel(0));
+ datafields["targetVel_y"] = new Variant(targetVel(1));
+ datafields["targetVel_z"] = new Variant(targetVel(2));
+
+ Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
+ datafields["adaptK_x"] = new Variant(adaptK(0));
+ datafields["adaptK_y"] = new Variant(adaptK(1));
+
+ datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
- // Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
- // datafields["targetVel_x"] = new Variant(targetVel(0));
- // datafields["targetVel_y"] = new Variant(targetVel(1));
- // datafields["targetVel_z"] = new Variant(targetVel(2));
// datafields["targetVel_rx"] = new Variant(targetVel(3));
// datafields["targetVel_ry"] = new Variant(targetVel(4));
// datafields["targetVel_rz"] = new Variant(targetVel(5));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index 634d20dd2..3c4d82ba3 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -107,6 +107,10 @@ namespace armarx
struct DebugRTData
{
Eigen::Matrix4f targetPose;
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f reactForce;
+ Eigen::Vector3f adaptK;
+ Eigen::VectorXf targetVel;
};
TripleBuffer<DebugRTData> debugRT;
@@ -168,6 +172,8 @@ namespace armarx
Eigen::Vector3f filteredForce;
Eigen::Vector3f forceOffset;
+ Eigen::Vector3f reactForce;
+ Eigen::Vector3f filteredForceInRoot;
Eigen::Matrix3f toolTransform;
Eigen::Vector3f oriToolDir;
--
GitLab