void RTKRobotArm::copy(const MathLib::Matrix& from, Eigen::MatrixXd& to) {
	int r = from.RowSize();
	int c = from.ColumnSize();
	for(int i=0; i<r; ++i) {
		for(int j=0; j<c; ++j) {
			to(i,j) = from.At(i,j);
		}
	}
}
/******************************************************************
 * Get jacobian full direction
 *
 * @ J : return direction jacobian T (9 X dof)
 ******************************************************************/
void sKinematics::getJacobianFullDirection(int axis1, int axis2, MathLib::Matrix &J)
{
	int lColSize = J.ColumnSize();
	MathLib::Matrix lJ3(3,lColSize);

	getJacobianPos(lJ3);
	J.SetRow(lJ3.GetRow(0), 0);
	J.SetRow(lJ3.GetRow(1), 1);
	J.SetRow(lJ3.GetRow(2), 2);

	getJacobianDirection(axis1, lJ3);
	J.SetRow(lJ3.GetRow(0), 3);
	J.SetRow(lJ3.GetRow(1), 4);
	J.SetRow(lJ3.GetRow(2), 5);

	getJacobianDirection(axis2, lJ3);
	J.SetRow(lJ3.GetRow(0), 6);
	J.SetRow(lJ3.GetRow(1), 7);
	J.SetRow(lJ3.GetRow(2), 8);
}
// Read sensor data and prepare SFinger data structure
void HandSkinControllerThread::PrepareSFingers(){

    bool bdm = bDebugMode;
    bDebugMode = false;

    if(bDebugMode){
        cout << "Input *************"<<endl;
    }

    // Position and default command
    for(int i=0;i<FINGERS_COUNT;i++){
        mSFingerPos[i][0] = mJointsPos[9+i*2 +0];
        mSFingerPos[i][1] = mJointsPos[9+i*2 +1];
        
        mSFingerCmd[i][0] = mJointsPos[9+i*2 +0];
        mSFingerCmd[i][1] = mJointsPos[9+i*2 +1];
    }

    static double badSkinVal[12];
    double tmpSkinVal[12];
    if(bBadSkin){
        // Let simulate some bad skin values from time to time
        //double period = 0.2;
        //double off = mTime/period - floor(mTime/period);
        //if(off < 0.05){
            for(int j=0;j<12;j++){
                badSkinVal[j] = 9.0+4.0*(rand()%1000)/1000.0;
            }
        //}
        
        for(int i=0;i<FINGERS_COUNT;i++){
            if(i==mBadFingerId){
                int offset = (i==0?4:i-1)*12;
                for(int j=0;j<12;j++){
                    tmpSkinVal[j] = mFingerTip[offset+j]; 
                    mFingerTip[offset+j] = badSkinVal[j];
                }
            }
        }
           
    }


    // Compute the average fingerTip pressure    
    for(int i=0;i<FINGERS_COUNT;i++){
        int offset = (i==0?4:i-1)*12;
        double val = 0.0;
        for(int j=0;j<12;j++){
            val += mFingerTip[offset+j];
        }
        val /= double(12);     
        mSFingerTip[i][0] = val;        
    }

    if(bDebugMode){
        cout << "Input Pressure:  "<< mSFingerTip[0][0]<<" "<<mSFingerTip[1][0]<<" "<<mSFingerTip[2][0]<<endl;
    }


    // FingerTip normal direction
    double vals[]={
      -1.00000,   0.00000,   0.00000,
      -0.39956,   0.00000,   0.91671,
      -0.39956,   0.00000,   0.91671,
      -1.00000,   0.00000,   0.00000,
      -0.78673,   0.60316,   0.13140,
      -0.30907,   0.47765,   0.82239,
      -0.00000,   1.00000,   0.00000,
       0.30907,   0.47765,   0.82239,
       0.78673,   0.60316,   0.13140,
       1.00000,   0.00000,   0.00000,
       0.39956,   0.00000,   0.91671,
       0.39956,   0.00000,   0.91671
    };
    MathLib::Matrix fingerTipPDir;
    fingerTipPDir.Set(vals,12,3);


    // Compute the normal direction
    for(int i=0;i<FINGERS_COUNT;i++){
        mSFingerTipDir[i].Zero();
        int offset = (i==0?4:i-1)*12;
        double val = 0.0;
        for(int j=0;j<12;j++){
            val               += mFingerTip[offset+j];
            mSFingerTipDir[i] += fingerTipPDir.GetRow(j) *(mFingerTip[offset+j]);
        }        
        if(val>1.0){
            mSFingerTipDir[i].Normalize();
        }else{
            mSFingerTipDir[i].Zero();
        }
    }


    if(bBadSkin){
        // Revert original skin values if necessary for data saving
        for(int i=0;i<FINGERS_COUNT;i++){
            if(i==mBadFingerId){
                int offset = (i==0?4:i-1)*12;
                for(int j=0;j<12;j++){
                    mFingerTip[offset+j] = tmpSkinVal[j];
                }
            }
        }
           
    }


    if(bDebugMode){
        cout << "Input Direction: "<< mSFingerTipDir[0]<<" "<<mSFingerTipDir[1]<<" "<<mSFingerTipDir[2]<<endl;
    }
    
    bDebugMode = bdm;
    return;

}