cob_forcetorque: ForceTorqueCtrl.cpp Source File

00001 #include <cob_forcetorque/ForceTorqueCtrl.h>
00002 #include <unistd.h>
00003 
00004 ForceTorqueCtrl::ForceTorqueCtrl()
00005 {
00006         out.open("force.txt"); 
00007 }
00008 
00009 ForceTorqueCtrl::~ForceTorqueCtrl()
00010 { 
00011 }
00012 
00013 bool ForceTorqueCtrl::Init()
00014 {
00015         initCan();
00016         //SetActiveCalibrationMatrix(0);
00017         //ReadCalibrationMatrix();
00018 }
00019 
00020 void ForceTorqueCtrl::initCan()
00021 {       
00022         std::cout << "initESDCan" << std::endl;
00023         m_Can = new CanESD("", false);
00024 }
00025 
00026 void ForceTorqueCtrl::ReadFTSerialNumber()
00027 {       
00028         std::cout << "\n\n*********CheckCalMatrix**********" << std::endl;
00029         CanMsg CMsg;
00030         CMsg.setID(0x205);
00031         CMsg.setLength(0);
00032 
00033         bool ret = m_Can->transmitMsg(CMsg, true);
00034 
00035         CanMsg replyMsg;
00036         replyMsg.set(0,0);
00037         replyMsg.set(0,1);
00038         replyMsg.set(0,2);
00039         replyMsg.set(0,3);
00040         replyMsg.set(0,4);
00041         bool ret2 = m_Can->receiveMsg(&replyMsg);
00042         int length = replyMsg.getLength();
00043         std::cout << "reply ID: \t" << replyMsg.getID()<<std::endl;
00044         std::cout << "reply Length: \t" << replyMsg.getLength()<<std::endl;
00045         std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " " 
00046                                       << replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " " 
00047                                       << replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " " 
00048                                       << replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
00049 }
00050 
00051 void ForceTorqueCtrl::SetActiveCalibrationMatrix(int num)
00052 {
00053         std::cout << "\n\n*******Setting Active Calibration Matrix Num to: "<< num <<"********"<< std::endl;
00054         BYTE b = 0;
00055         CanMsg CMsg;
00056         CMsg.setID(0x206);
00057         CMsg.setLength(1);
00058         CMsg.setAt(num,0);
00059 
00060         bool ret = m_Can->transmitMsg(CMsg, true);
00061 
00062         CanMsg replyMsg;
00063         bool ret2 = m_Can->receiveMsg(&replyMsg);
00064         if(ret2)
00065         {
00066                 std::cout<<"reply ID: \t"<<replyMsg.getID()<<std::endl;
00067                 std::cout<<"reply Length: \t"<<replyMsg.getLength()<<std::endl;
00068                 if(replyMsg.getID() == 0x206)
00069                 {
00070                         std::cout<<"Setting Calibration Matrix succeed!"<<std::endl;
00071                         std::cout<<"Calibration Matrix: "<<replyMsg.getAt(0)<<" is Activ!"<<std::endl;
00072                 }
00073                 else
00074                         std::cout<<"Error: Received wrong opcode!"<<std::endl;
00075         }
00076         else
00077                 std::cout<<"Error: Receiving Message failed!"<<std::endl;
00078 
00079 }
00080 
00081 
00082 void ForceTorqueCtrl::ReadCalibrationMatrix()
00083 {
00084         Eigen::VectorXf vCoef(6);
00085 
00086         //Read Fx coefficients
00087         ReadMatrix(0, vCoef);
00088         m_v3FXGain = vCoef;
00089 
00090         //Read Fy coefficients
00091         ReadMatrix(1, vCoef);
00092         m_v3FYGain = vCoef;
00093 
00094         //Read Fz coefficients
00095         ReadMatrix(2, vCoef);
00096         m_v3FZGain = vCoef;
00097 
00098         //Read Tx coefficients
00099         ReadMatrix(3, vCoef);
00100         m_v3TXGain = vCoef;
00101 
00102         //Read Ty coefficients
00103         ReadMatrix(4, vCoef);
00104         m_v3TYGain = vCoef;
00105 
00106         //Read Tz coefficients
00107         ReadMatrix(5, vCoef);
00108         m_v3TZGain = vCoef;
00109         SetCalibMatrix();
00110 }
00111 
00112 void ForceTorqueCtrl::ReadMatrix(int axis, Eigen::VectorXf& vec)
00113 {
00114         std::cout << "\n\n*******Read Matrix**********"<<std::endl;
00115         float statusCode = 0, sg0 = 0.0, sg1 = 0.0, sg2 = 0.0, sg3 = 0.0, sg4 = 0.0, sg5 = 0.0;
00116 
00117         CanMsg CMsg;
00118         CMsg.setID(0x202);
00119         CMsg.setLength(1);
00120         CMsg.setAt(axis,0);
00121 
00122         bool ret = m_Can->transmitMsg(CMsg, true);
00123         if(!ret)
00124         {
00125                 std::cout<<"Error: Requesting Calibration Matrix!"<<std::endl;
00126                 return;
00127         }
00128 
00129         CanMsg replyMsg;
00130         bool ret2 = m_Can->receiveMsg(&replyMsg);
00131         if(ret2)
00132         {
00133                 std::cout << "reply ID: \t" << replyMsg.getID()<<std::endl;
00134                 std::cout << "reply Length: \t" << replyMsg.getLength()<<std::endl;
00135                 std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " " 
00136                                       << replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " " 
00137                                       << replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " " 
00138                                       << replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
00139                 
00140                 fbBuf.bytes[0] = replyMsg.getAt(3);
00141                 fbBuf.bytes[1] = replyMsg.getAt(2);
00142                 fbBuf.bytes[2] = replyMsg.getAt(1);
00143                 fbBuf.bytes[3] = replyMsg.getAt(0);
00144                 sg0 = fbBuf.value;
00145                 
00146                 
00147                 fbBuf.bytes[0] = replyMsg.getAt(7);
00148                 fbBuf.bytes[1] = replyMsg.getAt(6);
00149                 fbBuf.bytes[2] = replyMsg.getAt(5);
00150                 fbBuf.bytes[3] = replyMsg.getAt(4);
00151                 sg1 = fbBuf.value;
00152 
00153         }
00154         else
00155                 return;
00156 
00157         ret2 = m_Can->receiveMsg(&replyMsg);
00158         if(ret2)
00159         {
00160                 std::cout << "reply ID: \t" << replyMsg.getID()<<std::endl;
00161                 std::cout << "reply Length: \t" << replyMsg.getLength()<<std::endl;
00162                 std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " " 
00163                                       << replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " " 
00164                                       << replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " " 
00165                                       << replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
00166 
00167                 fbBuf.bytes[0] = replyMsg.getAt(3);
00168                 fbBuf.bytes[1] = replyMsg.getAt(2);
00169                 fbBuf.bytes[2] = replyMsg.getAt(1);
00170                 fbBuf.bytes[3] = replyMsg.getAt(0);
00171                 sg2 = fbBuf.value;
00172                 
00173                 fbBuf.bytes[0] = replyMsg.getAt(7);
00174                 fbBuf.bytes[1] = replyMsg.getAt(6);
00175                 fbBuf.bytes[2] = replyMsg.getAt(5);
00176                 fbBuf.bytes[3] = replyMsg.getAt(4);
00177                 sg3 = fbBuf.value;
00178         }
00179         else
00180                 return;
00181 
00182         ret2 = m_Can->receiveMsg(&replyMsg);
00183         if(ret2)
00184         {
00185                 std::cout << "reply ID: \t" << replyMsg.getID()<<std::endl;
00186                 std::cout << "reply Length: \t" << replyMsg.getLength()<<std::endl;
00187                 std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " " 
00188                                       << replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " " 
00189                                       << replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " " 
00190                                       << replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
00191 
00192                 fbBuf.bytes[0] = replyMsg.getAt(3);
00193                 fbBuf.bytes[1] = replyMsg.getAt(2);
00194                 fbBuf.bytes[2] = replyMsg.getAt(1);
00195                 fbBuf.bytes[3] = replyMsg.getAt(0);
00196                 sg4 = fbBuf.value;
00197                 
00198                 fbBuf.bytes[0] = replyMsg.getAt(7);
00199                 fbBuf.bytes[1] = replyMsg.getAt(6);
00200                 fbBuf.bytes[2] = replyMsg.getAt(5);
00201                 fbBuf.bytes[3] = replyMsg.getAt(4);
00202                 sg5 = fbBuf.value;
00203         }
00204         else
00205                 return;
00206 
00207         vec[0] = sg0; vec[1] = sg1; vec[2] = sg2; vec[3] = sg3; vec[4] = sg4; vec[5] = sg5;
00208         std::cout<<"Matix:  SG0: "<<sg0<<" SG1: "<<sg1<<" SG2: "<<sg2<<" SG3: "<<sg3<<" SG4: "<<sg4<<" SG5: "<<sg5<<std::endl;
00209 }
00210 
00211 void ForceTorqueCtrl::ReadFirmwareVersion()
00212 {
00213         std::cout << "\n\n*******Reading Firmware Version: "<< std::endl;
00214         BYTE b = 0;
00215         CanMsg CMsg;
00216         CMsg.setID(0x20F);
00217         CMsg.setLength(0);
00218 
00219         bool ret = m_Can->transmitMsg(CMsg, true);
00220 
00221         CanMsg replyMsg;
00222         bool ret2 = m_Can->receiveMsg(&replyMsg);
00223         if(ret2)
00224         {
00225                 std::cout<<"reply ID: \t"<<replyMsg.getID()<<std::endl;
00226                 std::cout<<"reply Length: \t"<<replyMsg.getLength()<<std::endl;
00227                 if(replyMsg.getID() == 0x20F)
00228                 {
00229                         std::cout<<"Reading Firmware Succeed!"<<std::endl;
00230                         std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " " 
00231                                       << replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " " 
00232                                       << replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " " 
00233                                       << replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
00234                 }
00235                 else
00236                         std::cout<<"Error: Received wrong opcode!"<<std::endl;
00237         }
00238         else
00239                 std::cout<<"Error: Receiving Message failed!"<<std::endl;
00240 }
00241 
00242 void ForceTorqueCtrl::ReadSGData(double &Fx, double &Fy, double &Fz, double &Tx, double &Ty, double &Tz)
00243 {
00244         int statusCode = 0, sg0 = 0, sg1 = 0, sg2 = 0, sg3 = 0, sg4 = 0, sg5 = 0;
00245 
00246         CanMsg CMsg;
00247         CMsg.setID(0x200);
00248         CMsg.setLength(0);
00249 
00250         bool ret = m_Can->transmitMsg(CMsg, true);
00251 
00252         CanMsg replyMsg;
00253         bool ret2 = m_Can->receiveMsg(&replyMsg);
00254         unsigned char c[2];
00255         if(ret2)
00256         {
00257                 int length = replyMsg.getLength();
00258 
00259                 c[0] = replyMsg.getAt(0); //status code
00260                 c[1] = replyMsg.getAt(1);
00261                 //statusCode = (((char)c[0] << 8) | c[1]);
00262                 
00263                 c[0] = replyMsg.getAt(2); //sg0
00264                 c[1] = replyMsg.getAt(3);
00265 
00266                 //sg0 = (((char)c[0] << 8) | c[1]);
00267                 sg0 = (short)((c[0] << 8) | c[1]);
00268 
00269                 c[0] = replyMsg.getAt(4); //sg1
00270                 c[1] = replyMsg.getAt(5);
00271                 sg1 = (short)((c[0] << 8) | c[1]);
00272 
00273                 c[0] = replyMsg.getAt(6); //sg2
00274                 c[1] = replyMsg.getAt(7);
00275                 sg2 = (short)((c[0] << 8) | c[1]);
00276         }
00277         else
00278                 return;
00279 
00280         ret2 = m_Can->receiveMsg(&replyMsg);
00281         if(ret2)
00282         {
00283                 int length = replyMsg.getLength();
00284 
00285                 c[0] = replyMsg.getAt(0); //sg3
00286                 c[1] = replyMsg.getAt(1);
00287                 sg3 = (short)((c[0] << 8) | c[1]);
00288 
00289                 c[0] = replyMsg.getAt(2); //sg4
00290                 c[1] = replyMsg.getAt(3);
00291                 sg4 = (short)((c[0] << 8) | c[1]);
00292 
00293                 c[0] = replyMsg.getAt(4); //sg5
00294                 c[1] = replyMsg.getAt(5);
00295                 sg5 = (short)((c[0] << 8) | c[1]);
00296         }
00297         else
00298                 return;
00299 
00300 
00301         //std::cout<<"\nsg0: "<<sg0<<" sg1: "<<sg1<<" sg2: "<<sg2<<" sg3: "<<sg3<<" sg4: "<<sg4<<" sg5: "<<sg5<<std::endl;
00302         //out<<"sg0: "<<sg0<<" sg1: "<<sg1<<" sg2: "<<sg2<<" sg3: "<<sg3<<" sg4: "<<sg4<<" sg5: "<<sg5<<std::endl;
00303         
00304         StrainGaugeToForce(sg0, sg1, sg2, sg3, sg4, sg5);
00305         
00306         Fx = m_vForceData[0]; Fy = m_vForceData[1]; Fz = m_vForceData[2]; 
00307         Tx = m_vForceData[3]; Ty= m_vForceData[4]; Tz = m_vForceData[5];
00308         //out<<"Fx: "<<Fx<<" Fy: "<<Fy<<" Fz: "<<Fz<<" Tx: "<<Tx<<" Ty: "<<Ty<<" Tz: "<<Tz<<std::endl;
00309         
00310 }
00311 
00312 void ForceTorqueCtrl::StrainGaugeToForce(int& sg0, int& sg1, int& sg2, int& sg3, int& sg4, int& sg5)
00313 {
00314         Eigen::VectorXf v6SG(6);
00315         Eigen::VectorXf v6tmp(6);
00316         Eigen::VectorXf test(6);
00317         
00318         v6SG[0] = sg0; v6SG[1] = sg1; v6SG[2] = sg2; v6SG[3] = sg3; v6SG[4] = sg4; v6SG[5] = sg5;
00319         //v6SG[0] = 263; v6SG[1] = -272; v6SG[2] = -137; v6SG[3] = 9; v6SG[4] = 275; v6SG[5] = -258;
00320 
00321 /*      
00322         v6tmp = v6SG - m_v3StrainGaigeOffset;
00323         //std::cout<<"\nv6tmp: \n"<< v6tmp <<std::endl;
00324         //std::cout<<"\nCalibration Matrix: \n"<<m_mXCalibMatrix<<"\n\n";
00325         //std::cout<<"\nCalibration Matrix Transposed: \n"<<m_mXCalibMatrix.transpose()<<"\n\n";
00326 
00327         test = m_mXCalibMatrix * v6tmp;
00328         //std::cout << "Test: \n" << test << std::endl;
00329         m_vForceData = test;
00330 */
00331         //std::cout<<"\nCalibration Matrix: \n"<<m_mXCalibMatrix<<"\n\n";
00332         //std::cout<<"Calibration Matrix: rows: "<<m_mXCalibMatrix.rows()<<" columns: "<<m_mXCalibMatrix.cols()<<std::endl;
00333         test = m_mXCalibMatrix * v6SG;
00334         //std::cout<<"test: rows: "<<test.rows()<<" columns: "<<test.cols()<<std::endl;
00335         //std::cout<<"v6SG: rows: "<<v6SG.rows()<<" columns: "<<v6SG.cols()<<std::endl;
00336         m_vForceData = test * 0.000001;
00337         //std::cout << "test: \n" << m_vForceData << std::endl;
00338         //std::cout<<"m_vForceData: rows: "<<m_vForceData.rows()<<" columns: "<<m_vForceData.cols()<<std::endl;
00339 }
00340 
00341 void ForceTorqueCtrl::SetGaugeOffset(float sg0Off, float sg1Off, float sg2Off, float sg3Off, float sg4Off, float sg5Off)
00342 {
00343         Eigen::VectorXf tmp(6);
00344         tmp[0] = sg0Off; tmp[1] = sg1Off; tmp[2] = sg2Off; tmp[3] = sg3Off; tmp[4] = sg4Off; tmp[5] = sg5Off; 
00345         m_v3StrainGaigeOffset = tmp;
00346         //std::cout<<"GaugeOffset: \n"<<m_v3StrainGaigeOffset<<"\n";
00347         //std::cout<<"GaugeOffset 0: \n"<<tmp[0]<<"\n\n";
00348 }
00349 void ForceTorqueCtrl::SetGaugeGain(float gg0, float gg1, float gg2, float gg3, float gg4, float gg5)
00350 {
00351         Eigen::VectorXf tmp(6);
00352         tmp[0] = gg0; tmp[1] = gg1; tmp[2] = gg2; tmp[3] = gg3; tmp[4] = gg4; tmp[5] = gg5; 
00353         m_v3GaugeGain = tmp;
00354         //std::cout<<"GaugeGain: \n"<<m_v3GaugeGain<<"\n\n";
00355 }
00356 
00357 void ForceTorqueCtrl::SetFXGain(float fxg0, float fxg1, float fxg2, float fxg3, float fxg4, float fxg5)
00358 {
00359         Eigen::VectorXf tmp(6);
00360         tmp[0] = fxg0; tmp[1] = fxg1; tmp[2] = fxg2; tmp[3] = fxg3; tmp[4] = fxg4; tmp[5] = fxg5; 
00361         m_v3FXGain = tmp;
00362         //std::cout<<"FXGain: \n"<<m_v3FXGain<<"\n\n";
00363 }
00364 void ForceTorqueCtrl::SetFYGain(float fyg0, float fyg1, float fyg2, float fyg3, float fyg4, float fyg5)
00365 {
00366         Eigen::VectorXf tmp(6);
00367         tmp[0] = fyg0; tmp[1] = fyg1; tmp[2] = fyg2; tmp[3] = fyg3; tmp[4] = fyg4; tmp[5] = fyg5; 
00368         m_v3FYGain = tmp;
00369         //std::cout<<"FYGain: \n"<<m_v3FYGain<<"\n\n";
00370 
00371 }
00372 void ForceTorqueCtrl::SetFZGain(float fzg0, float fzg1, float fzg2, float fzg3, float fzg4, float fzg5)
00373 {
00374         Eigen::VectorXf tmp(6);
00375         tmp[0] = fzg0; tmp[1] = fzg1; tmp[2] = fzg2; tmp[3] = fzg3; tmp[4] = fzg4; tmp[5] = fzg5; 
00376         m_v3FZGain = tmp;
00377         //std::cout<<"FZGain: \n"<<m_v3FZGain<<"\n\n";
00378 
00379 }
00380 void ForceTorqueCtrl::SetTXGain(float txg0, float txg1, float txg2, float txg3, float txg4, float txg5)
00381 {
00382         Eigen::VectorXf tmp(6);
00383         tmp[0] = txg0; tmp[1] = txg1; tmp[2] = txg2; tmp[3] = txg3; tmp[4] = txg4; tmp[5] = txg5; 
00384         m_v3TXGain = tmp;
00385         //std::cout<<"TXGain: \n"<<m_v3TXGain<<"\n\n";
00386 }
00387 void ForceTorqueCtrl::SetTYGain(float tyg0, float tyg1, float tyg2, float tyg3, float tyg4, float tyg5)
00388 {
00389         Eigen::VectorXf tmp(6);
00390         tmp[0] = tyg0; tmp[1] = tyg1; tmp[2] = tyg2; tmp[3] = tyg3; tmp[4] = tyg4; tmp[5] = tyg5; 
00391         m_v3TYGain = tmp;
00392         //std::cout<<"TYGain: \n"<<m_v3TYGain<<"\n\n";
00393 }
00394 void ForceTorqueCtrl::SetTZGain(float tzg0, float tzg1, float tzg2, float tzg3, float tzg4, float tzg5)
00395 {
00396         Eigen::VectorXf tmp(6);
00397         tmp[0] = tzg0; tmp[1] = tzg1; tmp[2] = tzg2; tmp[3] = tzg3; tmp[4] = tzg4; tmp[5] = tzg5; 
00398         m_v3TZGain = tmp;
00399         //std::cout<<"TZGain: \n"<<m_v3TZGain<<"\n\n";
00400 }
00401 
00402 void ForceTorqueCtrl::CalcCalibMatrix()
00403 {
00404         Eigen::MatrixXf tmp(6, 6);
00405         tmp[0] = m_v3FXGain[0]/m_v3GaugeGain[0];
00406         tmp[1] = m_v3FXGain[1]/m_v3GaugeGain[1];
00407         tmp[2] = m_v3FXGain[2]/m_v3GaugeGain[2];
00408         tmp[3] = m_v3FXGain[3]/m_v3GaugeGain[3];
00409         tmp[4] = m_v3FXGain[4]/m_v3GaugeGain[4];
00410         tmp[5] = m_v3FXGain[5]/m_v3GaugeGain[5];
00411 
00412         tmp[6] = m_v3FYGain[0]/m_v3GaugeGain[0];
00413         tmp[7] = m_v3FYGain[1]/m_v3GaugeGain[1];
00414         tmp[8] = m_v3FYGain[2]/m_v3GaugeGain[2];
00415         tmp[9] = m_v3FYGain[3]/m_v3GaugeGain[3];
00416         tmp[10] = m_v3FYGain[4]/m_v3GaugeGain[4];
00417         tmp[11] = m_v3FYGain[5]/m_v3GaugeGain[5];
00418 
00419         tmp[12] = m_v3FZGain[0]/m_v3GaugeGain[0];
00420         tmp[13] = m_v3FZGain[1]/m_v3GaugeGain[1];
00421         tmp[14] = m_v3FZGain[2]/m_v3GaugeGain[2];
00422         tmp[15] = m_v3FZGain[3]/m_v3GaugeGain[3];
00423         tmp[16] = m_v3FZGain[4]/m_v3GaugeGain[4];
00424         tmp[17] = m_v3FZGain[5]/m_v3GaugeGain[5];
00425 
00426         tmp[18] = m_v3TXGain[0]/m_v3GaugeGain[0];
00427         tmp[19] = m_v3TXGain[1]/m_v3GaugeGain[1];
00428         tmp[20] = m_v3TXGain[2]/m_v3GaugeGain[2];
00429         tmp[21] = m_v3TXGain[3]/m_v3GaugeGain[3];
00430         tmp[22] = m_v3TXGain[4]/m_v3GaugeGain[4];
00431         tmp[23] = m_v3TXGain[5]/m_v3GaugeGain[5];
00432 
00433         tmp[24] = m_v3TYGain[0]/m_v3GaugeGain[0];
00434         tmp[25] = m_v3TYGain[1]/m_v3GaugeGain[1];
00435         tmp[26] = m_v3TYGain[2]/m_v3GaugeGain[2];
00436         tmp[27] = m_v3TYGain[3]/m_v3GaugeGain[3];
00437         tmp[28] = m_v3TYGain[4]/m_v3GaugeGain[4];
00438         tmp[29] = m_v3TYGain[5]/m_v3GaugeGain[5];
00439 
00440         tmp[30] = m_v3TZGain[0]/m_v3GaugeGain[0];
00441         tmp[31] = m_v3TZGain[1]/m_v3GaugeGain[1];
00442         tmp[32] = m_v3TZGain[2]/m_v3GaugeGain[2];
00443         tmp[33] = m_v3TZGain[3]/m_v3GaugeGain[3];
00444         tmp[34] = m_v3TZGain[4]/m_v3GaugeGain[4];
00445         tmp[35] = m_v3TZGain[5]/m_v3GaugeGain[5];
00446         
00447         m_mXCalibMatrix = tmp;
00448                         
00449 }
00450 void ForceTorqueCtrl::SetCalibMatrix()
00451 {
00452         Eigen::MatrixXf tmp(6, 6);
00453         tmp[0] = m_v3FXGain[0];
00454         tmp[1] = m_v3FXGain[1];
00455         tmp[2] = m_v3FXGain[2];
00456         tmp[3] = m_v3FXGain[3];
00457         tmp[4] = m_v3FXGain[4];
00458         tmp[5] = m_v3FXGain[5];
00459 
00460         tmp[6] = m_v3FYGain[0];
00461         tmp[7] = m_v3FYGain[1];
00462         tmp[8] = m_v3FYGain[2];
00463         tmp[9] = m_v3FYGain[3];
00464         tmp[10] = m_v3FYGain[4];
00465         tmp[11] = m_v3FYGain[5];
00466 
00467         tmp[12] = m_v3FZGain[0];
00468         tmp[13] = m_v3FZGain[1];
00469         tmp[14] = m_v3FZGain[2];
00470         tmp[15] = m_v3FZGain[3];
00471         tmp[16] = m_v3FZGain[4];
00472         tmp[17] = m_v3FZGain[5];
00473 
00474         tmp[18] = m_v3TXGain[0];
00475         tmp[19] = m_v3TXGain[1];
00476         tmp[20] = m_v3TXGain[2];
00477         tmp[21] = m_v3TXGain[3];
00478         tmp[22] = m_v3TXGain[4];
00479         tmp[23] = m_v3TXGain[5];
00480 
00481         tmp[24] = m_v3TYGain[0];
00482         tmp[25] = m_v3TYGain[1];
00483         tmp[26] = m_v3TYGain[2];
00484         tmp[27] = m_v3TYGain[3];
00485         tmp[28] = m_v3TYGain[4];
00486         tmp[29] = m_v3TYGain[5];
00487 
00488         tmp[30] = m_v3TZGain[0];
00489         tmp[31] = m_v3TZGain[1];
00490         tmp[32] = m_v3TZGain[2];
00491         tmp[33] = m_v3TZGain[3];
00492         tmp[34] = m_v3TZGain[4];
00493         tmp[35] = m_v3TZGain[5];
00494 
00495         
00496         m_mXCalibMatrix = tmp.transpose();
00497                         
00498 }