From 7476cae373b5232a261227683bb1054db6e2bd3e Mon Sep 17 00:00:00 2001
From: zhou <you.zhou@kit.edu>
Date: Wed, 19 Feb 2020 19:27:21 +0100
Subject: [PATCH] modified the bimanual controller
---
.../NJointBimanualForceController.cpp | 83 ++++++++++++-------
.../NJointBimanualForceController.h | 16 ++--
2 files changed, 62 insertions(+), 37 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
index bcdc2f58a..bee6f061d 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
@@ -188,6 +188,12 @@ namespace armarx
initSensorData.currentTwist.setZero();
controllerSensorData.reinitAllBuffers(initSensorData);
+
+ NJointBimanualForceControllerInterfaceData initInterfaceData;
+ initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
+ initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
+ interfaceData.reinitAllBuffers(initInterfaceData);
+
leftInitialPose = boxInitialPose;
leftInitialPose(0, 3) -= cfg->boxWidth;
rightInitialPose = boxInitialPose;
@@ -233,6 +239,15 @@ namespace armarx
ARMARX_IMPORTANT << "finished construction!";
dmpStarted = false;
+
+ targetWrench.setZero(cfg->targetWrench.size());
+ for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
+ {
+ targetWrench(i) = cfg->targetWrench[i];
+ }
+
+
+
}
std::string NJointBimanualForceController::getClassName(const Ice::Current&) const
@@ -264,6 +279,11 @@ namespace armarx
Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
+ if (objectDMP->canVal < 1e-8)
+ {
+ finished = true;
+ }
+
objectDMP->flow(deltaT, currentPose, currentTwist);
LockGuardType guard {controlDataMutex};
@@ -305,6 +325,16 @@ namespace armarx
rToBoxCoM << cfg->boxWidth, 0, 0;
Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
+
+ interfaceData.getWriteBuffer().currentLeftPose = currentLeftPose;
+ interfaceData.getWriteBuffer().currentRightPose = currentRightPose;
+ interfaceData.commitWrite();
+
+ Eigen::VectorXf currentTargetWrench = targetWrench;
+ if (fabs(currentLeftPose(0, 3) - currentRightPose(0, 3)) < 0.8 * 2 * cfg->boxWidth)
+ {
+ currentTargetWrench.setZero();
+ }
currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
Eigen::MatrixXf graspMatrix;
@@ -460,8 +490,8 @@ namespace armarx
// }
for (size_t i = 0; i < 6; i++)
{
- forcePID(i) = cfg->forceP[i] * (cfg->targetWrench[i] - wrenchesMeasured(i));
- forcePID(i + 6) = cfg->forceP[i] * (cfg->targetWrench[i + 6] - wrenchesMeasured(i + 6));
+ forcePID(i) = cfg->forceP[i] * (currentTargetWrench(i) - wrenchesMeasured(i));
+ forcePID(i + 6) = cfg->forceP[i] * (currentTargetWrench(i + 6) - wrenchesMeasured(i + 6));
}
Eigen::VectorXf forcePIDInRoot(12);
forcePIDInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(0, 0);
@@ -557,6 +587,7 @@ namespace armarx
Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
+
// torque limit
for (size_t i = 0; i < leftTargets.size(); ++i)
{
@@ -642,30 +673,6 @@ namespace armarx
debugOutputData.getWriteBuffer().forcePID = forcePIDInRootForDebug;
- // debugOutputData.getWriteBuffer().rightControlSignal_x = rightJointControlWrench(0);
- // debugOutputData.getWriteBuffer().rightControlSignal_y = rightJointControlWrench(1);
- // debugOutputData.getWriteBuffer().rightControlSignal_z = rightJointControlWrench(2);
- // debugOutputData.getWriteBuffer().rightControlSignal_ro = rightJointControlWrench(3);
- // debugOutputData.getWriteBuffer().rightControlSignal_pi = rightJointControlWrench(4);
- // debugOutputData.getWriteBuffer().rightControlSignal_ya = rightJointControlWrench(5);
-
- // debugOutputData.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
- // debugOutputData.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
- // debugOutputData.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
- // debugOutputData.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
- // debugOutputData.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
- // debugOutputData.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
-
-
- // debugOutputData.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
- // debugOutputData.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
- // debugOutputData.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
-
- // debugOutputData.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
- // debugOutputData.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
- // debugOutputData.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
- // debugOutputData.getWriteBuffer().virtualTime = virtualtime;
-
debugOutputData.commitWrite();
}
@@ -686,8 +693,29 @@ namespace armarx
void NJointBimanualForceController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
{
+ while (!interfaceData.updateReadBuffer())
+ {
+ usleep(1000);
+ }
+
+ Eigen::Matrix4f leftPose = interfaceData.getUpToDateReadBuffer().currentLeftPose;
+ Eigen::Matrix4f rightPose = interfaceData.getUpToDateReadBuffer().currentRightPose;
+
+ VirtualRobot::MathTools::Quaternion boxOri = VirtualRobot::MathTools::eigen4f2quat(leftPose);
+ Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
+
+ std::vector<double> boxInitialPose;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ boxInitialPose.push_back(boxPosi(i) * 0.001); //Important: mm -> m
+ }
+ boxInitialPose.push_back(boxOri.w);
+ boxInitialPose.push_back(boxOri.x);
+ boxInitialPose.push_back(boxOri.y);
+ boxInitialPose.push_back(boxOri.z);
+
virtualtimer = cfg->timeDuration;
- objectDMP->prepareExecution(cfg->boxInitialPose, goals);
+ objectDMP->prepareExecution(boxInitialPose, goals);
finished = false;
dmpStarted = true;
@@ -834,7 +862,6 @@ namespace armarx
void NJointBimanualForceController::onDisconnectNJointController()
{
- controllerTask->stop();
ARMARX_INFO << "stopped ...";
}
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h
index 9d6de20f7..80a97c1fe 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h
@@ -78,7 +78,7 @@ namespace armarx
void controllerRun();
private:
-
+ Eigen::VectorXf targetWrench;
struct DebugBufferData
{
StringFloatDictionary desired_torques;
@@ -137,13 +137,13 @@ namespace armarx
};
TripleBuffer<NJointBimanualForceControllerSensorData> controllerSensorData;
- // struct NJointBimanualForceControllerInterfaceData
- // {
- // Eigen::Matrix4f currentLeftPose;
- // Eigen::Matrix4f currentRightPose;
- // };
+ struct NJointBimanualForceControllerInterfaceData
+ {
+ Eigen::Matrix4f currentLeftPose;
+ Eigen::Matrix4f currentRightPose;
+ };
- // TripleBuffer<NJointBimanualForceControllerInterfaceData> interfaceData;
+ TripleBuffer<NJointBimanualForceControllerInterfaceData> interfaceData;
std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
@@ -169,8 +169,6 @@ namespace armarx
double virtualtimer;
mutable MutexType controllerMutex;
- PeriodicTask<NJointBimanualForceController>::pointer_type controllerTask;
-
Eigen::VectorXf leftDesiredJointValues;
Eigen::VectorXf rightDesiredJointValues;
--
GitLab