extended code by blending function, needs testing whether working correctly

c++/include/yumi.hpp

@@ -26,6 +26,7 @@ class Yumi {
     void set_kp(double kp);
     void set_operationPoint(double op);
     void set_hybridControl(bool hybridControl);
+    void set_transitionTime(double transitionTime);
 
     void process();
 
@@ -54,6 +55,10 @@ class Yumi {
     // gain for control
     double m_kp = 1.0;
 
+    // time for transition between position control to force control in the wires axis
+    double m_transitionTime = 0.0;
+    double m_deltaTime = 0.0; 
+
     // vars to store configuration
     rl::math::Vector3 m_desPosition = rl::math::Vector3::Zero();
     rl::math::Vector3 m_desPositionDot = rl::math::Vector3::Zero();
@@ -84,6 +89,7 @@ class Yumi {
     Eigen::Matrix3d euler2rotMatrix(rl::math::Vector3 orientation);
     void compTaskSpaceInput();
     void compForce2VelocityController();
+    void modifySelectionMatrix();
 
     
  };
\ No newline at end of file

c++/src/py2gpmbind.cpp

@@ -21,5 +21,6 @@ PYBIND11_MODULE(invKin, m) {
        .def("set_kp", &Yumi::set_kp)
        .def("set_operationPoint", &Yumi::set_operationPoint)
        .def("set_hybridControl", &Yumi::set_hybridControl)
+       .def("set_transitionTime", &Yumi::set_transitionTime)
        .def("set_force", &Yumi::set_force);
 	}
\ No newline at end of file

c++/src/yumi.cpp

@@ -86,6 +86,7 @@ void Yumi::compTaskSpaceInput(){
 void Yumi::process(){
 
     doForwardKinematics();
+    modifySelectionMatrix();
     compForce2VelocityController();
     compTaskSpaceInput();
 
@@ -123,6 +124,7 @@ void Yumi::compForce2VelocityController(){
     // have a look at chosen ee frame in RS
     velocityEE << 0, m_force-m_forceOP, 0;
     velocityEE *= m_kp;
+    velocityEE =  (Eigen::Matrix3d::Identity() - m_selectVelMatrix) * velocityEE; // perform blending - transition from position control to force control
     // transform the velocities computed in the ee frame to the world frame
     m_forceTaskSpaceInput = m_rotationMatrix.transpose() * velocityEE;
 }
@@ -143,9 +145,28 @@ void Yumi::set_hybridControl(bool hybridControl){
                                     0, 0, 0, 
                                     0, 0, 1;
                                     
-        } else {
+        } else { // position control
             m_selectVelMatrix = Eigen::Matrix3d::Identity();
+            m_deltaTime = 0.0; // reset variable to apply blending when changing to hybrid control again
         }
     }
     m_hybridControl = hybridControl;
 }
+
+void Yumi::set_transitionTime(double transitionTime){
+    m_transitionTime = transitionTime;
+}
+
+void Yumi::modifySelectionMatrix(){
+    Eigen::Matrix3d blendingMatrix;
+    if(m_deltaTime < m_transitionTime){
+        m_deltaTime += 1.0/m_sampleTime;
+        blendingMatrix <<   0, 0, 0,
+                            0, (1 - m_deltaTime/m_transitionTime), 0,
+                            0, 0, 0; 
+    }
+    else {
+        blendingMatrix = Eigen::Matrix3d::Zero();
+    }
+    m_selectVelMatrix += blendingMatrix; 
+}
\ No newline at end of file