worked on bimanualobjlevelcontroller

source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice

@@ -96,13 +96,16 @@ module armarx
 
         Ice::FloatSeq forceThreshold;
 
+        double ftCalibrationTime;
+
     };
 
     interface NJointBimanualObjLevelControllerInterface extends NJointControllerInterface
     {
         void learnDMPFromFiles(Ice::StringSeq trajfiles);
         bool isFinished();
-        void runDMP(Ice::DoubleSeq goals);
+        void runDMP(Ice::DoubleSeq goals, double timeDuration);
+        void runDMPWithVirtualStart(Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
 
 //        void setViaPoints(double canVal, Ice::DoubleSeq point);
         void setGoals(Ice::DoubleSeq goals);

source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h

@@ -191,6 +191,8 @@ namespace armarx
         void learnDMPFromSampledTrajectory(const DMP::Vec<DMP::SampledTrajectoryV2>& trajs, const std::vector<double>& ratios);
 
         void setRatios(const std::vector<double>& ratios);
+        TaskSpaceDMPControllerConfig config;
+
     private:
 
         double tau;
@@ -201,7 +203,6 @@ namespace armarx
 
         DMP::UMITSMPPtr dmpPtr;
         DMP::Vec<DMP::DMPState > currentState;
-        TaskSpaceDMPControllerConfig config;
 
 
         bool isDisturbance;

source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp

@@ -104,7 +104,7 @@ namespace armarx
         tcpLeft = leftRNS->getTCP();
         tcpRight = rightRNS->getTCP();
 
-        KpImpedance.resize(cfg->KpImpedance.size());
+        KpImpedance.setZero(cfg->KpImpedance.size());
         ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 12);
 
         for (int i = 0; i < KpImpedance.size(); ++i)
@@ -112,7 +112,7 @@ namespace armarx
             KpImpedance(i) = cfg->KpImpedance.at(i);
         }
 
-        KdImpedance.resize(cfg->KdImpedance.size());
+        KdImpedance.setZero(cfg->KdImpedance.size());
         ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 12);
 
         for (int i = 0; i < KdImpedance.size(); ++i)
@@ -120,7 +120,7 @@ namespace armarx
             KdImpedance(i) = cfg->KdImpedance.at(i);
         }
 
-        KpAdmittance.resize(cfg->KpAdmittance.size());
+        KpAdmittance.setZero(cfg->KpAdmittance.size());
         ARMARX_CHECK_EQUAL(cfg->KpAdmittance.size(), 6);
 
         for (int i = 0; i < KpAdmittance.size(); ++i)
@@ -128,7 +128,7 @@ namespace armarx
             KpAdmittance(i) = cfg->KpAdmittance.at(i);
         }
 
-        KdAdmittance.resize(cfg->KdAdmittance.size());
+        KdAdmittance.setZero(cfg->KdAdmittance.size());
         ARMARX_CHECK_EQUAL(cfg->KdAdmittance.size(), 6);
 
         for (int i = 0; i < KdAdmittance.size(); ++i)
@@ -136,7 +136,7 @@ namespace armarx
             KdAdmittance(i) = cfg->KdAdmittance.at(i);
         }
 
-        KmAdmittance.resize(cfg->KmAdmittance.size());
+        KmAdmittance.setZero(cfg->KmAdmittance.size());
         ARMARX_CHECK_EQUAL(cfg->KmAdmittance.size(), 6);
 
         for (int i = 0; i < KmAdmittance.size(); ++i)
@@ -245,6 +245,10 @@ namespace armarx
 
         dmpStarted = false;
 
+
+        ftcalibrationTimer = 0;
+        ftOffset.setZero(12);
+
         targetWrench.setZero(cfg->targetWrench.size());
         for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
         {
@@ -284,10 +288,12 @@ namespace armarx
         double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
         Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
         Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
+        ARMARX_IMPORTANT << "canVal:  " << objectDMP->canVal;
 
         if (objectDMP->canVal < 1e-8)
         {
             finished = true;
+            dmpStarted = false;
         }
 
         objectDMP->flow(deltaT, currentPose, currentTwist);
@@ -303,6 +309,15 @@ namespace armarx
 
     void NJointBimanualObjLevelController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
     {
+        Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
+        Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
+
+        controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
+        controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
+        controlInterfaceBuffer.commitWrite();
+        double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+        ftcalibrationTimer += deltaT;
         if (firstLoop)
         {
             Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
@@ -326,7 +341,26 @@ namespace armarx
             ARMARX_INFO << "modified left pose:\n " << leftPose;
             ARMARX_INFO << "modified right pose:\n " << rightPose;
         }
-        double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+        if (ftcalibrationTimer < cfg->ftCalibrationTime)
+        {
+            ftOffset.block<3, 1>(0, 0) = 0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
+            ftOffset.block<3, 1>(3, 0) = 0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
+            ftOffset.block<3, 1>(6, 0) = 0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
+            ftOffset.block<3, 1>(9, 0) = 0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
+
+            for (int i = 0; i < KmAdmittance.size(); ++i)
+            {
+                KmAdmittance(i) = 0;
+            }
+        }
+        else
+        {
+            for (int i = 0; i < KmAdmittance.size(); ++i)
+            {
+                KmAdmittance(i) = cfg->KmAdmittance.at(i);
+            }
+        }
 
         // -------------------------------------------- target wrench ---------------------------------------------
         Eigen::VectorXf deltaPoseForWrenchControl;
@@ -339,12 +373,7 @@ namespace armarx
 
         // ------------------------------------------- current tcp pose -------------------------------------------
 
-        Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
-        Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
 
-        controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
-        controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
-        controlInterfaceBuffer.commitWrite();
 
         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);
@@ -657,7 +686,7 @@ namespace armarx
     }
 
 
-    void NJointBimanualObjLevelController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
+    void NJointBimanualObjLevelController::runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
     {
         while (!controlInterfaceBuffer.updateReadBuffer())
         {
@@ -670,6 +699,8 @@ namespace armarx
         VirtualRobot::MathTools::Quaternion boxOri = VirtualRobot::MathTools::eigen4f2quat(leftPose);
         Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
 
+        ARMARX_IMPORTANT << "runDMP: boxPosi: " << boxPosi;
+
         std::vector<double> boxInitialPose;
         for (size_t i = 0; i < 3; ++i)
         {
@@ -680,8 +711,25 @@ namespace armarx
         boxInitialPose.push_back(boxOri.y);
         boxInitialPose.push_back(boxOri.z);
 
-        virtualtimer = cfg->timeDuration;
         objectDMP->prepareExecution(boxInitialPose, goals);
+        objectDMP->canVal = timeDuration;
+        objectDMP->config.motionTimeDuration = timeDuration;
+
+
+        finished = false;
+        dmpStarted = true;
+    }
+
+    void NJointBimanualObjLevelController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
+    {
+        while (!controlInterfaceBuffer.updateReadBuffer())
+        {
+            usleep(1000);
+        }
+
+        objectDMP->prepareExecution(starts, goals);
+        objectDMP->canVal = timeDuration;
+        objectDMP->config.motionTimeDuration = timeDuration;
 
         finished = false;
         dmpStarted = true;

source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h

@@ -65,7 +65,9 @@ namespace armarx
         }
 
         //        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
-        void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
+        void runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
+        void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
+
         void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
         void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
         double getVirtualTime(const Ice::Current&)
@@ -252,6 +254,8 @@ namespace armarx
         Eigen::VectorXf filteredOldValue;
         bool finished;
         bool dmpStarted;
+        double ftcalibrationTimer;
+        Eigen::VectorXf ftOffset;
     protected:
         void rtPreActivateController();
         bool firstLoop;

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

@@ -442,6 +442,8 @@ namespace armarx
         dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
 
         dmpCtrl->canVal = timeDuration;
+        dmpCtrl->config.motionTimeDuration = timeDuration;
+
         finished = false;
 
         if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)