disabled drift compensation in force controlled axis

c++/include/yumi.hpp

@@ -25,6 +25,7 @@ class Yumi {
     void set_force(double force);
     void set_kp(double kp);
     void set_operationPoint(double op);
+    void set_hybridControl(bool hybridControl);
 
     void process();
 
@@ -38,6 +39,9 @@ class Yumi {
     // vars for rl library
     std::shared_ptr<rl::mdl::Model> m_model;
 
+    // flag for hybrid control
+    bool m_hybridControl = false; 
+
     // vars for tuning
     double m_driftCompGain = 1.0;
     double m_sampleTime = 0.0125;
@@ -73,6 +77,8 @@ class Yumi {
 
     Eigen::Matrix<double, 7, 1> m_jointAnglesDelta;
 
+    Eigen::Matrix3d m_selectVelMatrix = Eigen::Matrix3d::Identity();
+
     // private functions
     void doForwardKinematics();
     Eigen::Matrix3d euler2rotMatrix(rl::math::Vector3 orientation);

c++/src/py2gpmbind.cpp

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

c++/src/yumi.cpp

@@ -77,6 +77,9 @@ void Yumi::compTaskSpaceInput(){
 	Eigen::Quaterniond errorQuaternion = currentOrientation.inverse() * desiredOrientation;
 	Eigen::Vector3d errorRotationInWorldFrame = currentOrientation * errorQuaternion.vec();
 
+    m_desPosition = m_selectVelMatrix * m_desPosition + (Eigen::Matrix3d::Identity() - m_selectVelMatrix) * m_position;
+    m_desPositionDot = m_selectVelMatrix * m_desPositionDot;
+
     m_effectiveTaskSpaceInput.head(3) = m_driftCompGain/m_sampleTime * (m_desPosition - m_position)
 										+ m_desPositionDot + m_forceTaskSpaceInput;
 	m_effectiveTaskSpaceInput.tail(3) = m_driftCompGain/m_sampleTime * errorRotationInWorldFrame + m_desOrientationDot;
@@ -112,8 +115,10 @@ Eigen::Matrix<double, 6, 1> Yumi::get_newPose(){
 }
 
 void Yumi::set_force(double force){
+    if (m_hybridControl){
         m_force = force;
     }
+}
 
 void Yumi::compForce2VelocityController(){
     Eigen::Vector3d velocityEE;
@@ -132,3 +137,18 @@ void Yumi::set_kp(double kp){
 void Yumi::set_operationPoint(double op){
     m_forceOP = op;
 }
+
+void Yumi::set_hybridControl(bool hybridControl){
+    if (hybridControl != m_hybridControl){
+        m_hybridControl = hybridControl;
+        if (hybridControl){
+            m_selectVelMatrix <<    1, 0, 0,    // y direction is force controlled in this case
+                                    0, 0, 0, 
+                                    0, 0, 1;
+                                    
+        } else {
+            m_selectVelMatrix = Eigen::Matrix3d::Identity();
+        }
+    }
+    m_hybridControl = hybridControl;
+}
\ No newline at end of file

python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_realSensor.py

@@ -40,8 +40,9 @@ desPose_R_const = np.concatenate((trans_const, rot_const), axis=0)
 egm_client_R = EGMClient(port=UDP_PORT_RIGHT)
 
 yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
-yumi_right.set_operationPoint(0.5)
-yumi_right.set_kp(3.65)
+yumi_right.set_operationPoint(0.7)
+yumi_right.set_kp(0.2)
+yumi_right.set_hybridControl(True)
 
 desVelocities_R_const = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
 jointVelocities_R = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
@@ -83,7 +84,7 @@ while True and arduino.isOpen():
     if arduino.in_waiting: # get the number of bytes in the input buffer
         packet = arduino.readline() # type: bytes  
         str_receive = packet.decode('utf-8').rstrip('\n')
-        force = float(str_receive)
+        force = float(str_receive)/1000.0 # mN to Newtion
 
     if (time.time() - timestamp) >= (1.0/rate):
         # get the current joints angles for the right arm