implemented bimanualccdmp impedance control

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp

@@ -37,6 +37,9 @@ namespace armarx
 
         };
 
+//        ARMARX_CHECK_EQUAL(leftVelocitySensors, 8);
+//        ARMARX_CHECK_EQUAL(leftPositionSensors, 8);
+
         VirtualRobot::RobotNodeSetPtr rightRNS = robotUnit->getRtRobot()->getRobotNodeSet("RightArm");
         for (size_t i = 0; i < rightRNS->getSize(); ++i)
         {
@@ -61,6 +64,8 @@ namespace armarx
             rightPositionSensors.push_back(positionSensor);
         };
 
+//        ARMARX_CHECK_EQUAL(rightVelocitySensors, 8);
+//        ARMARX_CHECK_EQUAL(rightPositionSensors, 8);
 
         leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
         rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
@@ -341,23 +346,25 @@ namespace armarx
 
     Eigen::VectorXf NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf &tcptwist, const Eigen::Matrix4f &currentPose, 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 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::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::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;
 
-        Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
-        Eigen::Vector6f tcpDesiredWrench;
-        tcpDesiredWrench <<   0.001 * tcpDesiredForce, tcpDesiredTorque;
+//        return tcpDesiredWrench;
 
     }
 
@@ -407,13 +414,52 @@ namespace armarx
 
         Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
         Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
+        Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
+        Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
 
-        Eigen::VectorXf leftJointControlWrench = getControlWrench(currentLeftTwist, tcpLeft->getPoseInRootFrame(), leftTargetPose);
-        Eigen::VectorXf rightJointControlWrench = getControlWrench(currentRightTwist, tcpRight->getPoseInRootFrame(), rightTargetPose);
+        Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
+        Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
 
 
-        Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
+        Eigen::Vector6f leftJointControlWrench;
+        {
+            Eigen::Vector3f targetTCPLinearVelocity;
+            targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1), 0.001 * leftTargetVel(2);
+
+            Eigen::Vector3f currentTCPLinearVelocity;
+            currentTCPLinearVelocity << 0.001 * currentLeftTwist(0),   0.001 * currentLeftTwist(1), 0.001 * currentLeftTwist(2);
+            Eigen::Vector3f currentTCPAngularVelocity;
+            currentTCPAngularVelocity << currentLeftTwist(3),   currentLeftTwist(4),  currentLeftTwist(5);
+            Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3,1>(0,3);
+            Eigen::Vector3f desiredPosition = leftTargetPose.block<3,1>(0,3);
+
+            Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+            Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
+            Eigen::Matrix3f diffMat = leftTargetPose.block<3,3>(0,0) * currentRotMat.inverse();
+            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+            Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+            leftJointControlWrench <<   0.001 * tcpDesiredForce, tcpDesiredTorque;
+        }
+
+
+        Eigen::Vector6f rightJointControlWrench;
+        {
+            Eigen::Vector3f currentTCPLinearVelocity;
+            currentTCPLinearVelocity << 0.001 * currentRightTwist(0),   0.001 * currentRightTwist(1), 0.001 * currentRightTwist(2);
+            Eigen::Vector3f currentTCPAngularVelocity;
+            currentTCPAngularVelocity << currentRightTwist(3),   currentRightTwist(4),  currentRightTwist(5);
+            Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3,1>(0,3);
+            Eigen::Vector3f desiredPosition = rightTargetPose.block<3,1>(0,3);
+
+            Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) - dpos.cwiseProduct(currentTCPLinearVelocity);
+            Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
+            Eigen::Matrix3f diffMat = rightTargetPose.block<3,3>(0,0) * currentRotMat.inverse();
+            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+            Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+            rightJointControlWrench <<   0.001 * tcpDesiredForce, tcpDesiredTorque;
+        }
 
+        Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
         Eigen::VectorXf rightNullspaceTorque = knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
 
         float lambda = 2;
@@ -458,6 +504,23 @@ namespace armarx
 //        debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
 //        debugDataInfo.getWriteBuffer().quatError = errorAngle;
 //        debugDataInfo.getWriteBuffer().posiError = posiError;
+        debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0,3);
+        debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1,3);
+        debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2,3);
+        debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0,3);
+        debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1,3);
+        debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2,3);
+
+
+        debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0,3);
+        debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1,3);
+        debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2,3);
+
+        debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0,3);
+        debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1,3);
+        debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2,3);
+
+
         debugDataInfo.commitWrite();
 
 
@@ -614,6 +677,21 @@ namespace armarx
             datafields[pair.first] = new Variant(pair.second);
         }
 
+        datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
+        datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
+        datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
+        datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
+        datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
+        datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
+
+
+        datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
+        datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
+        datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
+        datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
+        datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
+        datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
+
 //        StringVariantBaseMap datafields;
 //        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
 //        for (auto& pair : values)

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h

@@ -30,8 +30,8 @@ namespace armarx
         Eigen::VectorXf leftTargetVel;
         Eigen::VectorXf rightTargetVel;
 
-        Eigen::MatrixXf leftTargetPose;
-        Eigen::MatrixXf rightTargetPose;
+        Eigen::Matrix4f leftTargetPose;
+        Eigen::Matrix4f rightTargetPose;
     };
 
     class NJointBimanualCCDMPController :
@@ -103,6 +103,20 @@ namespace armarx
         struct DebugBufferData
         {
             StringFloatDictionary desired_torques;
+            float leftTargetPose_x;
+            float leftTargetPose_y;
+            float leftTargetPose_z;
+            float rightTargetPose_x;
+            float rightTargetPose_y;
+            float rightTargetPose_z;
+
+            float leftCurrentPose_x;
+            float leftCurrentPose_y;
+            float leftCurrentPose_z;
+            float rightCurrentPose_x;
+            float rightCurrentPose_y;
+            float rightCurrentPose_z;
+
 //            StringFloatDictionary latestTargetVelocities;
 //            StringFloatDictionary dmpTargets;
 //            StringFloatDictionary currentPose;