From 68373a3915617dbf983eb5211ef3d29ee902bf1b Mon Sep 17 00:00:00 2001
From: Armar6 <armar6@h2t.com>
Date: Mon, 30 Apr 2018 17:13:48 +0200
Subject: [PATCH] added torque-based bimanual
---
.../NJointTaskSpaceDMPController.ice | 11 +-
.../NJointBimanualCCDMPController.cpp | 339 ++++++++++++------
.../NJointBimanualCCDMPController.h | 56 ++-
3 files changed, 272 insertions(+), 134 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 3f289eb02..545f7f774 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -156,7 +156,6 @@ module armarx
Ice::FloatSeq leftDesiredJointValues;
Ice::FloatSeq rightDesiredJointValues;
- double knull = 5;
float KoriFollower = 1;
float KposFollower = 1;
@@ -164,6 +163,16 @@ module armarx
double maxLinearVel;
double maxAngularVel;
+ Ice::FloatSeq Kpos;
+ Ice::FloatSeq Dpos;
+ Ice::FloatSeq Kori;
+ Ice::FloatSeq Dori;
+
+ float knull;
+ float dnull;
+
+ float torqueLimit;
+
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
index 2d00b4b95..c28eedd39 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -16,9 +16,10 @@ namespace armarx
for (size_t i = 0; i < leftRNS->getSize(); ++i)
{
std::string jointName = leftRNS->getNode(i)->getName();
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ leftJointNames.push_back(jointName);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
const SensorValueBase* sv = prov->getSensorValue(jointName);
- leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
if (!velocitySensor)
@@ -40,9 +41,10 @@ namespace armarx
for (size_t i = 0; i < rightRNS->getSize(); ++i)
{
std::string jointName = rightRNS->getNode(i)->getName();
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ rightJointNames.push_back(jointName);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
const SensorValueBase* sv = prov->getSensorValue(jointName);
- rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
@@ -129,6 +131,16 @@ namespace armarx
KoriFollower = cfg->KoriFollower;
KposFollower = cfg->KposFollower;
+
+ 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];
+
+ knull = cfg->knull;
+ dnull = cfg->dnull;
+
+ torqueLimit = cfg->torqueLimit;
}
std::string NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
@@ -216,6 +228,8 @@ namespace armarx
std::vector<double> followerTargetPoseVec = followerDMP->eigen4f2vec(followerTargetPose);
+ Eigen::Matrix4f leftTargetPose;
+ Eigen::Matrix4f rightTargetPose;
if (leaderName == "Left")
{
@@ -225,6 +239,9 @@ namespace armarx
leftDMPTarget = leaderDMP->getTargetPose();
rightDMPTarget = followerTargetPoseVec;
+
+ leftTargetPose = leaderTargetPose;
+ rightTargetPose = followerTargetPose;
}
else
{
@@ -234,76 +251,79 @@ namespace armarx
rightDMPTarget = leaderDMP->getTargetPose();
leftDMPTarget = followerTargetPoseVec;
+ rightTargetPose = leaderTargetPose;
+ leftTargetPose = followerTargetPose;
+
}
- debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_xvel"] = leftTargetVel(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yvel"] = leftTargetVel(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_zvel"] = leftTargetVel(2);
- debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_rollvel"] = leftTargetVel(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_pitchvel"] = leftTargetVel(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yawvel"] = leftTargetVel(5);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_xvel"] = leftTargetVel(0);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yvel"] = leftTargetVel(1);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_zvel"] = leftTargetVel(2);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_rollvel"] = leftTargetVel(3);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_pitchvel"] = leftTargetVel(4);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yawvel"] = leftTargetVel(5);
- debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_xvel"] = rightTargetVel(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yvel"] = rightTargetVel(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_zvel"] = rightTargetVel(2);
- debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_rollvel"] = rightTargetVel(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_pitchvel"] = rightTargetVel(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yawvel"] = rightTargetVel(5);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_xvel"] = rightTargetVel(0);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yvel"] = rightTargetVel(1);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_zvel"] = rightTargetVel(2);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_rollvel"] = rightTargetVel(3);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_pitchvel"] = rightTargetVel(4);
+// debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yawvel"] = rightTargetVel(5);
- debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_x"] = leftDMPTarget[0];
- debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_y"] = leftDMPTarget[1];
- debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_z"] = leftDMPTarget[2];
- debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qw"] = leftDMPTarget[3];
- debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qx"] = leftDMPTarget[4];
- debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qy"] = leftDMPTarget[5];
- debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qz"] = leftDMPTarget[6];
+// debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_x"] = leftDMPTarget[0];
+// debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_y"] = leftDMPTarget[1];
+// debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_z"] = leftDMPTarget[2];
+// debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qw"] = leftDMPTarget[3];
+// debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qx"] = leftDMPTarget[4];
+// debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qy"] = leftDMPTarget[5];
+// debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qz"] = leftDMPTarget[6];
- debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_x"] = rightDMPTarget[0];
- debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_y"] = rightDMPTarget[1];
- debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_z"] = rightDMPTarget[2];
- debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qw"] = rightDMPTarget[3];
- debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qx"] = rightDMPTarget[4];
- debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qy"] = rightDMPTarget[5];
- debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qz"] = rightDMPTarget[6];
+// debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_x"] = rightDMPTarget[0];
+// debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_y"] = rightDMPTarget[1];
+// debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_z"] = rightDMPTarget[2];
+// debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qw"] = rightDMPTarget[3];
+// debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qx"] = rightDMPTarget[4];
+// debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qy"] = rightDMPTarget[5];
+// debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qz"] = rightDMPTarget[6];
- std::vector<double> currentLeftPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentLeftPose);
- std::vector<double> currentRightPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentRightPose);
+// std::vector<double> currentLeftPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentLeftPose);
+// std::vector<double> currentRightPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentRightPose);
- debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = currentLeftPoseVec[0];
- debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = currentLeftPoseVec[1];
- debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = currentLeftPoseVec[2];
- debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = currentLeftPoseVec[3];
- debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = currentLeftPoseVec[4];
- debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = currentLeftPoseVec[5];
- debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = currentLeftPoseVec[6];
+// debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = currentLeftPoseVec[0];
+// debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = currentLeftPoseVec[1];
+// debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = currentLeftPoseVec[2];
+// debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = currentLeftPoseVec[3];
+// debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = currentLeftPoseVec[4];
+// debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = currentLeftPoseVec[5];
+// debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = currentLeftPoseVec[6];
- debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = currentRightPoseVec[0];
- debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = currentRightPoseVec[1];
- debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = currentRightPoseVec[2];
- debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = currentRightPoseVec[3];
- debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = currentRightPoseVec[4];
- debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = currentRightPoseVec[5];
- debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = currentRightPoseVec[6];
+// debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = currentRightPoseVec[0];
+// debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = currentRightPoseVec[1];
+// debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = currentRightPoseVec[2];
+// debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = currentRightPoseVec[3];
+// debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = currentRightPoseVec[4];
+// debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = currentRightPoseVec[5];
+// debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = currentRightPoseVec[6];
- debugOutputData.getWriteBuffer().leaderCanVal = leaderDMP->debugData.canVal;
- debugOutputData.getWriteBuffer().leadermpcFactor = leaderDMP->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().leadererror = leaderDMP->debugData.poseError;
- debugOutputData.getWriteBuffer().leaderposError = leaderDMP->debugData.posiError;
- debugOutputData.getWriteBuffer().leaderoriError = leaderDMP->debugData.oriError;
+// debugOutputData.getWriteBuffer().leaderCanVal = leaderDMP->debugData.canVal;
+// debugOutputData.getWriteBuffer().leadermpcFactor = leaderDMP->debugData.mpcFactor;
+// debugOutputData.getWriteBuffer().leadererror = leaderDMP->debugData.poseError;
+// debugOutputData.getWriteBuffer().leaderposError = leaderDMP->debugData.posiError;
+// debugOutputData.getWriteBuffer().leaderoriError = leaderDMP->debugData.oriError;
- debugOutputData.getWriteBuffer().followerCanVal = followerDMP->debugData.canVal;
- debugOutputData.getWriteBuffer().followermpcFactor = followerDMP->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().followererror = followerDMP->debugData.poseError;
- debugOutputData.getWriteBuffer().followerposError = followerDMP->debugData.posiError;
- debugOutputData.getWriteBuffer().followeroriError = followerDMP->debugData.oriError;
+// debugOutputData.getWriteBuffer().followerCanVal = followerDMP->debugData.canVal;
+// debugOutputData.getWriteBuffer().followermpcFactor = followerDMP->debugData.mpcFactor;
+// debugOutputData.getWriteBuffer().followererror = followerDMP->debugData.poseError;
+// debugOutputData.getWriteBuffer().followerposError = followerDMP->debugData.posiError;
+// debugOutputData.getWriteBuffer().followeroriError = followerDMP->debugData.oriError;
- debugOutputData.commitWrite();
+// debugOutputData.commitWrite();
@@ -314,10 +334,33 @@ namespace armarx
LockGuardType guard {controlDataMutex};
getWriterControlStruct().leftTargetVel = leftTargetVel;
getWriterControlStruct().rightTargetVel = rightTargetVel;
-
+ getWriterControlStruct().leftTargetPose = leftTargetPose;
+ getWriterControlStruct().rightTargetPose = rightTargetPose;
writeControlStruct();
}
+ Eigen::VectorXf NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf &tcptwist, const Eigen::Matrix4f ¤tPose, const Eigen::Matrix4f &targetPose)
+ {
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
+
+ Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
+ Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
+ Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
+ Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
+
+ Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
+ Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ Eigen::Vector6f tcpDesiredWrench;
+ tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
+
+ }
+
void NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
@@ -353,54 +396,112 @@ namespace armarx
rightqvel(i) = rightVelocitySensors[i]->velocity;
}
- controllerSensorData.getWriteBuffer().currentLeftTwist = jacobiL * leftqvel;
- controllerSensorData.getWriteBuffer().currentRightTwist = jacobiR * rightqvel;
+ Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
+ Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
+
+
+ controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
+ controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
controllerSensorData.commitWrite();
- Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
- float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
- if (normLinearVelocity > cfg->maxLinearVel)
- {
- leftTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- }
- float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
- if (normAngularVelocity > cfg->maxAngularVel)
- {
- leftTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- }
- Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
- Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
- Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
+ Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
+ Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
+
+ Eigen::VectorXf leftJointControlWrench = getControlWrench(currentLeftTwist, tcpLeft->getPoseInRootFrame(), leftTargetPose);
+ Eigen::VectorXf rightJointControlWrench = getControlWrench(currentRightTwist, tcpRight->getPoseInRootFrame(), rightTargetPose);
+
+
+ Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
+
+ Eigen::VectorXf rightNullspaceTorque = knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
+
+ float lambda = 2;
+ Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
+ Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
+
+ Eigen::MatrixXf jtpinvR = leftIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
+ Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
+
for (size_t i = 0; i < leftTargets.size(); ++i)
{
- leftTargets.at(i)->velocity = jnvL(i);
- }
+ float desiredTorque = leftJointDesiredTorques(i);
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
- Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
- normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
- if (normLinearVelocity > cfg->maxLinearVel)
- {
- rightTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- }
- normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
- if (normAngularVelocity > cfg->maxAngularVel)
- {
- rightTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- }
+ debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
+ leftTargets.at(i)->torque = desiredTorque;
+ }
- Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
- Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
- Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
for (size_t i = 0; i < rightTargets.size(); ++i)
{
- rightTargets.at(i)->velocity = jnvR(i);
+ float desiredTorque = rightJointDesiredTorques(i);
+
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+
+ debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
+
+ rightTargets.at(i)->torque = desiredTorque;
}
+
+// debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
+// debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
+// debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
+// debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
+// debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
+// debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
+// debugDataInfo.getWriteBuffer().quatError = errorAngle;
+// debugDataInfo.getWriteBuffer().posiError = posiError;
+ debugDataInfo.commitWrite();
+
+
+
+// Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
+// float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
+// if (normLinearVelocity > cfg->maxLinearVel)
+// {
+// leftTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+// }
+// float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
+// if (normAngularVelocity > cfg->maxAngularVel)
+// {
+// leftTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+// }
+
+// Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
+// Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
+// Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
+
+// for (size_t i = 0; i < leftTargets.size(); ++i)
+// {
+// leftTargets.at(i)->velocity = jnvL(i);
+// }
+
+// Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
+// normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
+// if (normLinearVelocity > cfg->maxLinearVel)
+// {
+// rightTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+// }
+// normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
+// if (normAngularVelocity > cfg->maxAngularVel)
+// {
+// rightTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+// }
+// Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
+// Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
+// Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
+
+// for (size_t i = 0; i < rightTargets.size(); ++i)
+// {
+// rightTargets.at(i)->velocity = jnvR(i);
+// }
}
void NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
@@ -505,36 +606,44 @@ namespace armarx
void NJointBimanualCCDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
+
StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
for (auto& pair : values)
{
datafields[pair.first] = new Variant(pair.second);
}
- auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- for (auto& pair : dmpTargets)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- for (auto& pair : currentPose)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- datafields["leaderCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderCanVal);
- datafields["leadermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadermpcFactor);
- datafields["leaderError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadererror);
- datafields["leaderposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderposError);
- datafields["leaderoriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderoriError);
-
- datafields["followerCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerCanVal);
- datafields["followermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().followermpcFactor);
- datafields["followerError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followererror);
- datafields["followerposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerposError);
- datafields["followeroriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followeroriError);
+// StringVariantBaseMap datafields;
+// auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+// for (auto& pair : values)
+// {
+// datafields[pair.first] = new Variant(pair.second);
+// }
+
+// auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+// for (auto& pair : dmpTargets)
+// {
+// datafields[pair.first] = new Variant(pair.second);
+// }
+
+// auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+// for (auto& pair : currentPose)
+// {
+// datafields[pair.first] = new Variant(pair.second);
+// }
+
+// datafields["leaderCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderCanVal);
+// datafields["leadermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadermpcFactor);
+// datafields["leaderError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadererror);
+// datafields["leaderposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderposError);
+// datafields["leaderoriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderoriError);
+
+// datafields["followerCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerCanVal);
+// datafields["followermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().followermpcFactor);
+// datafields["followerError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followererror);
+// datafields["followerposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerposError);
+// datafields["followeroriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followeroriError);
debugObs->setDebugChannel("DMPController", datafields);
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
index 39a1a6e84..fbc9e03ed 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
@@ -29,6 +29,9 @@ namespace armarx
public:
Eigen::VectorXf leftTargetVel;
Eigen::VectorXf rightTargetVel;
+
+ Eigen::MatrixXf leftTargetPose;
+ Eigen::MatrixXf rightTargetPose;
};
class NJointBimanualCCDMPController :
@@ -77,6 +80,9 @@ namespace armarx
void onDisconnectComponent();
void controllerRun();
private:
+
+ Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose);
+
Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
{
@@ -96,24 +102,27 @@ namespace armarx
struct DebugBufferData
{
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary dmpTargets;
- StringFloatDictionary currentPose;
-
- double leadermpcFactor;
- double leadererror;
- double leaderposError;
- double leaderoriError;
- double leaderCanVal;
-
- double followermpcFactor;
- double followererror;
- double followerposError;
- double followeroriError;
- double followerCanVal;
+ StringFloatDictionary desired_torques;
+// StringFloatDictionary latestTargetVelocities;
+// StringFloatDictionary dmpTargets;
+// StringFloatDictionary currentPose;
+
+// double leadermpcFactor;
+// double leadererror;
+// double leaderposError;
+// double leaderoriError;
+// double leaderCanVal;
+
+// double followermpcFactor;
+// double followererror;
+// double followerposError;
+// double followeroriError;
+// double followerCanVal;
+
+
};
- TripleBuffer<DebugBufferData> debugOutputData;
+ TripleBuffer<DebugBufferData> debugDataInfo;
struct NJointBimanualCCDMPControllerSensorData
{
@@ -127,11 +136,11 @@ namespace armarx
TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
- std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
+ std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
- std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
+ std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
@@ -160,7 +169,18 @@ namespace armarx
float DposFollower;
float DoriFollower;
+ Eigen::Vector3f kpos;
+ Eigen::Vector3f kori;
+ Eigen::Vector3f dpos;
+ Eigen::Vector3f dori;
+
+ float knull;
+ float dnull;
+
+ std::vector<std::string> leftJointNames;
+ std::vector<std::string> rightJointNames;
+ float torqueLimit;
// NJointBimanualCCDMPControllerInterface interface
};
--
GitLab