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; }