rtest.cpp
Go to the documentation of this file.
00001 /*
00002 ROBOOP -- A robotics object oriented package in C++
00003 Copyright (C) 1996-2004  Richard Gourdeau  and Etienne Lachance
00004 
00005 This library is free software; you can redistribute it and/or modify
00006 it under the terms of the GNU Lesser General Public License as
00007 published by the Free Software Foundation; either version 2.1 of the
00008 License, or (at your option) any later version.
00009 
00010 This library is distributed in the hope that it will be useful,
00011 but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 GNU Lesser General Public License for more details.
00014 
00015 You should have received a copy of the GNU Lesser General Public
00016 License along with this library; if not, write to the Free Software
00017 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018 
00019 
00020 Report problems and direct all questions to:
00021 
00022 Richard Gourdeau
00023 Professeur Agrege
00024 Departement de genie electrique
00025 Ecole Polytechnique de Montreal
00026 C.P. 6079, Succ. Centre-Ville
00027 Montreal, Quebec, H3C 3A7
00028 
00029 email: richard.gourdeau@polymtl.ca
00030 
00031 -------------------------------------------------------------------------------
00032 Revision_history:
00033 
00034 2004/07/01: Ethan Tira-Thompson
00035     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00036 
00037 2004/08/13: Etienne Lachance
00038     -Modified robot initialisation matrix.
00039 -------------------------------------------------------------------------------
00040 */
00041 
00049 
00050 static const char rcsid[] = "$Id: rtest.cpp,v 1.15 2005/07/01 17:44:53 gourdeau Exp $";
00051 
00052 #include "robot.h"
00053 #include "quaternion.h"
00054 #include "precisio.h"
00055 #include <fstream>
00056 
00057 #ifdef use_namespace
00058 using namespace ROBOOP;
00059 #endif
00060 
00061 const Real PUMA560_data_DH[] =
00062   {0, 0, 0, 0, M_PI/2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.35, 0, 0, 0, 
00063    0, 0, 0, 0.4318, 0, 0, 0, 0, 17.4, -0.3638, 0.006, 0.2275, 0.13, 0, 0, 0.524, 0, 0.539, 0, 
00064    0, 0, 0.15005, 0.0203, -M_PI/2.0, 0, 0, 0, 4.8, -0.0203, -0.0141, 0.07, 0.066, 0, 0, 0.086, 0, 0.0125, 0,
00065    0, 0, 0.4318, 0.0, M_PI/2.0, 0, 0, 0, 0.82, 0, 0.019, 0, 0.0018, 0, 0, 0.0013, 0, 0.0018, 0, 
00066    0, 0, 0, 0.0, -M_PI/2.0, 0, 0, 0, 0.34, 0.0, 0.0, 0.0, 0.0003, 0.0, 0.0, 0.0004, 0.0, 0.0003, 0, 
00067    0, 0, 0, 0, 0, 0, 0, 0, 0.09, 0.0, 0.0, 0.032, 0.00015, 0.0, 0.0, 0.00015, 0.0, 0.00004, 0};
00068 Real PUMA560_data_mDH[] =
00069    //joint_type, theta, d, a, alpha
00070 {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.35, 0,
00071  0, 0, 0, 0.0, -M_PI/2, 0, 0, 0, 17.4, 0.068, 0.006, -0.016, 0.13, 0, 0, 0.524, 0, 0.539, 0,
00072  0, 0, -0.15005, 0.4318, 0, 0, 0, 0, 4.8, 0, -0.070, 0.014, 0.066, 0, 0, 0.0125, 0, 0.066, 0,
00073  0, 0, -0.4318, 0.0203, -M_PI/2.0, 0, 0, 0, 0.82, 0.0, 0.0, -0.019, 0.0018, 0, 0, 0.0018, 0, 0.0013, 0,
00074  0, 0, 0, 0, M_PI/2.0, 0, 0, 0, 0.34, 0, 0, 0.0, 0.0003, 0, 0, 0.0003, 0, 0.0004, 0,
00075  0, 0, 0, 0, -M_PI/2, 0, 0, 0, 0.09, 0, 0, 0.032, 0.00015, 0, 0, 0.00015, 0, 0.00004, 0};
00076 const Real PUMA560_motor[] =
00077    {200e-6, -62.6111, 1.48e-3, 0, // using + and - directions average
00078     200e-6, 107.815, .817e-3, 0,
00079     200e-6, -53.7063, 1.38e-3, 0,
00080     33e-6,  76.0364, 71.2e-6, 0,
00081     33e-6,  71.923,  82.6e-6, 0,
00082     33e-6,  76.686,  36.7e-6, 0};
00083 
00084 const Real STANFORD_data_DH[] =
00085   {0.0, 0.0, 0.4120, 0.0, -M_PI/2, 0,0,0,9.29, 0.0, 0.0175, -0.1105, 0.276, 0.0, 0, 0.255, 0.0, 0.071,0,0,0,0,0,
00086    0.0, 0.0, 0.1540, 0.0, M_PI/2.0, 0,0,0,5.01, 0.0, -1.054, 0.0, 0.108, 0.0, 0.0, 0.018, 0.0, 0.1,0,0,0,0,0,
00087    1.0, -M_PI/2.0, 0.0, 0.0, 0.0, 0,0,0,4.25, 0.0, 0.0, -6.447, 2.51, 0.0, 0.0, 2.51, 0.0, 0.006,0,0,0,0,0,
00088    0.0, 0.0, 0.0, 0.0, -M_PI/2.0, 0,0,0,1.08, 0.0, 0.092, -0.054, 0.002, 0.0, 0.0, 0.001, 0.0, 0.001,0,0,0,0,0,
00089    0.0, 0.0, 0.0, 0.0,  M_PI/2.0, 0,0,0,0.63, 0.0, 0.0, 0.566, 0.003, 0.0, 0.0, 0.003, 0.0, 0.0004,0,0,0,0,0,
00090    0.0, 0.0, 0.2630, 0.0, 0.0, 0,0,0,0.51, 0.0, 0.0, 1.5540, 0.013, 0.0, 0.0, 0.013, 0.0, 0.0003,0,0,0,0,0};
00091 
00092 int main(void)
00093 {
00094    int i;
00095    std::ifstream infile;
00096    infile.open("source/test.txt");
00097    if(!infile) {
00098       cerr << "Cannot open file:";
00099       cerr << "source/test.txt\n";
00100       exit (1);
00101    }
00102    Matrix Test(4,4), p1(3,1);
00103    Real a, eps = FloatingPointPrecision::Epsilon(); /* floating point eps */
00104 
00105    cout << "=====================================================\n";
00106    cout << " ROBOOP -- A robotics object oriented package in C++ \n";;
00107    cout << " TEST program \n";
00108    cout << "=====================================================\n";
00109    cout << "\n";
00110    cout << "Machine epsilon = " << eps << endl;
00111    eps *= 150.0;
00112 
00113    cout << "Testing translation : ";
00114    Real ott[] = {1.0,2.0,3.0};
00115    p1 << ott;
00116    for(i = 1; i <= 4; i++) {
00117       for(int j = 1; j <= 4; j++) {
00118          infile >> Test(i,j);
00119       }
00120    }
00121    a = (Test-trans(p1)).MaximumAbsoluteValue();
00122    if(a > eps) {
00123       cout << "Erreur = " << a << endl;
00124    } else {
00125       cout << "Ok" << endl;
00126    }
00127 
00128    cout << "Testing rotx : ";
00129    for(i = 1; i <= 4; i++) {
00130       for(int j = 1; j <= 4; j++) {
00131          infile >> Test(i,j);
00132       }
00133    }
00134    a = (Test-rotx(M_PI/6)).MaximumAbsoluteValue();
00135    if(a > eps) {
00136       cout << "Erreur = " << a << endl;
00137    } else {
00138       cout << "Ok" << endl;
00139    }
00140 
00141    cout << "Testing roty : ";
00142    for(i = 1; i <= 4; i++) {
00143       for(int j = 1; j <= 4; j++) {
00144          infile >> Test(i,j);
00145       }
00146    }
00147    a = (Test-roty(M_PI/6)).MaximumAbsoluteValue();
00148    if(a > eps) {
00149       cout << "Erreur = " << a << endl;
00150    } else {
00151       cout << "Ok" << endl;
00152    }
00153 
00154    cout << "Testing rotz : ";
00155    for(i = 1; i <= 4; i++) {
00156       for(int j = 1; j <= 4; j++) {
00157          infile >> Test(i,j);
00158       }
00159    }
00160    a = (Test-rotz(M_PI/6)).MaximumAbsoluteValue();
00161    if(a > eps) {
00162       cout << "Erreur = " << a << endl;
00163    } else {
00164       cout << "Ok" << endl;
00165    }
00166 
00167    cout << "Testing roll-pitch-yaw : ";
00168    for(i = 1; i <= 4; i++) {
00169       for(int j = 1; j <= 4; j++) {
00170          infile >> Test(i,j);
00171       }
00172    }
00173    a = (Test-rpy(p1)).MaximumAbsoluteValue();
00174    if(a > eps) {
00175       cout << "Erreur = " << a << endl;
00176    } else {
00177       cout << "Ok" << endl;
00178    }
00179 
00180    cout << "Testing rotation around vector : ";
00181    for(i = 1; i <= 4; i++) {
00182       for(int j = 1; j <= 4; j++) {
00183          infile >> Test(i,j);
00184       }
00185    }
00186    a = (Test-rotk(M_PI/2,p1)).MaximumAbsoluteValue();
00187    if(a > eps) {
00188       cout << "Erreur = " << a << endl;
00189    } else {
00190       cout << "Ok" << endl;
00191    }
00192 
00193    // R(z) with angle=pi/4.
00194    cout << "Testing quaternion to rotation matrix : ";
00195    for(i = 1; i <= 3; i++) {
00196       for(int j = 1; j <= 3; j++) {
00197          infile >> Test(i,j);
00198       }
00199    }
00200    {
00201       //     quaternion from angle + vector
00202       ColumnVector v(3); v(1)=v(2)=0.0; v(3)=1.0;
00203       Quaternion q(M_PI/4, v);
00204       a = (Test.SubMatrix(1,3,1,3)-q.R()).MaximumAbsoluteValue();
00205       if(a > eps) {
00206          cout << "Erreur = " << a << endl;
00207       } else {
00208          cout << "Ok" << endl;
00209       }
00210    }
00211 
00212    cout << "Testing quaternion to rotation matrix : ";
00213    for(i = 1; i <= 3; i++) {
00214       for(int j = 1; j <= 3; j++) {
00215          infile >> Test(i,j);
00216       }
00217    }
00218 
00219    {
00220       // quaternion from 4 parameters
00221       Quaternion q(M_PI/4, M_PI/6, M_PI/8, 1);
00222       q.unit();
00223       a = (Test.SubMatrix(1,3,1,3)-q.R()).MaximumAbsoluteValue();
00224       if(a > eps) {
00225          cout << "Erreur = " << a << endl;
00226       } else {
00227          cout << "Ok" << endl;
00228       }
00229 
00230       cout << "Testing quaternion to transformation matrix : ";
00231       for(i = 1; i <= 4; i++) {
00232          for(int j = 1; j <= 4; j++) {
00233             infile >> Test(i,j);
00234          }
00235       }
00236       a = (Test-q.T()).MaximumAbsoluteValue();
00237       if(a > eps) {
00238          cout << "Erreur = " << a << endl;
00239       } else {
00240          cout << "Ok" << endl;
00241       }
00242 
00243       cout << "Testing rotation matrix to quaternion : ";
00244       ColumnVector quat(4);
00245       Quaternion qq(q.R());
00246       Test = Matrix(4,1);
00247       for(i = 1; i <= 4; i++) {
00248          infile >> Test(i,1);
00249       }
00250       quat(1) = qq.s();
00251       quat.SubMatrix(2,4,1,1) = qq.v();
00252       a = (Test-quat).MaximumAbsoluteValue();
00253       if(a > eps) {
00254          cout << "Erreur = " << a << endl;
00255       } else {
00256          cout << "Ok" << endl;
00257       }
00258 
00259       cout << "Testing transformation matrix to quaternion : ";
00260       qq = Quaternion(q.T());
00261       Test = Matrix(4,1);
00262       for(i = 1; i <= 4; i++) {
00263          infile >> Test(i,1);
00264       }
00265       quat(1) = qq.s();
00266       quat.SubMatrix(2,4,1,1) = qq.v();
00267       a = (Test-quat).MaximumAbsoluteValue();
00268       if(a > eps) {
00269          cout << "Erreur = " << a << endl;
00270       } else {
00271          cout << "Ok" << endl;
00272       }
00273    }
00274 
00275 
00276    // ---------------------- R O B O T S -------------------------------------
00277    Robot robot_DH;
00278    mRobot robot_mDH;
00279    Matrix initrob;
00280    Matrix initrobm;
00281    short dof;
00282 
00283    ColumnVector qr, q, qd, qdd;
00284    ColumnVector Fext(3), Next(3);
00285 
00286    // Puma 560 in DH notation without motor
00287    cout << "Testing Puma 560 (DH) forward kinematic : ";
00288    Test = Matrix(4,4);
00289    for(i = 1; i <= 4; i++) {
00290       for(int j = 1; j <= 4; j++) {
00291          infile >> Test(i,j);
00292       }
00293    }
00294    initrob = Matrix(6,19);
00295    initrobm = Matrix(6,4);
00296    initrob << PUMA560_data_DH;
00297    initrobm << PUMA560_motor;
00298    robot_DH = Robot(initrob, initrobm);
00299    dof = robot_DH.get_dof();
00300    qr = ColumnVector(dof);
00301    qr = M_PI/4.0;
00302    robot_DH.set_q(qr);
00303    a = (Test-robot_DH.kine()).MaximumAbsoluteValue();
00304    if(a > eps) {
00305       cout << "Erreur = " << a << endl;
00306    } else {
00307       cout << "Ok" << endl;
00308    }
00309 
00310    cout << "Testing Puma 560 (DH) jacobian in base frame : ";
00311    Test = Matrix(6,6);
00312    for(i = 1; i <= 6; i++) {
00313       for(int j = 1; j <= 6; j++) {
00314          infile >> Test(i,j);
00315       }
00316    }
00317    a = (Test-robot_DH.jacobian()).MaximumAbsoluteValue();
00318    if(a > eps) {
00319       cout << "Erreur = " << a << endl;
00320    } else {
00321       cout << "Ok" << endl;
00322    }
00323 
00324    cout << "Testing Puma 560 (DH) jacobian in tool frame : ";
00325    for(i = 1; i <= 6; i++) {
00326       for(int j = 1; j <= 6; j++) {
00327          infile >> Test(i,j);
00328       }
00329    }
00330    a = (Test-robot_DH.jacobian(dof)).MaximumAbsoluteValue();
00331    if(a > eps) {
00332       cout << "Erreur = " << a << endl;
00333    } else {
00334       cout << "Ok" << endl;
00335    }
00336 
00337    initrobm = Matrix(6,4);
00338    initrobm = 0.0;
00339    robot_DH = Robot(initrob,initrobm);
00340    Test = Matrix(dof,1); Test = 0;
00341    cout << "Testing Puma 560 (DH) torque : ";
00342    for(i = 1; i <= dof; i++) {
00343       infile >> Test(i,1);
00344    }
00345 
00346    qd = qr;
00347    qdd = qr;
00348    a = (Test-robot_DH.torque(qr, qd, qdd)).MaximumAbsoluteValue();
00349    if(a > eps) {
00350       cout << "Erreur = " << a << endl;
00351    } else {
00352       cout << "Ok" << endl;
00353    }
00354 
00355    cout << "Testing Puma 560 (DH) inertia : ";
00356    Test = Matrix(6,6);
00357    for(i = 1; i <= 6; i++) {
00358       for(int j = 1; j <= 6; j++){
00359          infile >> Test(i,j);
00360       }
00361    }
00362    a = (Test-robot_DH.inertia(qr)).MaximumAbsoluteValue();
00363    if(a > eps) {
00364       cout << "Erreur = " << a << endl;
00365    } else {
00366       cout << "Ok" << endl;
00367    }
00368 
00369    initrobm = Matrix(6,4);
00370    initrobm << PUMA560_motor;
00371    robot_DH = Robot(initrob,initrobm);
00372    dof = robot_DH.get_dof();
00373    
00374    cout << "Testing Puma 560 (DH) motor, torque : ";
00375    Test = Matrix(dof,1);
00376    for(i = 1; i <= dof; i++) {
00377       infile >> Test(i,1);
00378    }
00379    a = (Test-robot_DH.torque(qr, qd, qdd)).MaximumAbsoluteValue();
00380    if(a > eps) {
00381       cout << "Erreur = " << a << endl;
00382    } else {
00383       cout << "Ok" << endl;
00384    }
00385 
00386    // Stanford in DH notation
00387    cout << "Testing Stanford (DH) forward kinematic : ";
00388    Test = Matrix(4,4);
00389    for(i = 1; i <= 4; i++) {
00390       for(int j = 1; j <= 4; j++) {
00391          infile >> Test(i,j);
00392       }
00393    }
00394    initrob = Matrix(6,23);
00395    initrob << STANFORD_data_DH;
00396    robot_DH = Robot(initrob);
00397    dof = robot_DH.get_dof();
00398    qr = ColumnVector(dof);
00399    qr = M_PI/4.0;
00400    robot_DH.set_q(qr);
00401    a = (Test-robot_DH.kine()).MaximumAbsoluteValue();
00402    if(a > eps) {
00403       cout << "Erreur = " << a << endl;
00404    } else {
00405       cout << "Ok" << endl;
00406    }
00407 
00408    cout << "Testing Stanford (DH) jacobian in base frame : ";
00409    Test = Matrix(6,6);
00410    for(i = 1; i <= 6; i++) {
00411       for(int j = 1; j <= 6; j++) {
00412          infile >> Test(i,j);
00413       }
00414    }
00415    a = (Test-robot_DH.jacobian()).MaximumAbsoluteValue();
00416    if(a > eps) {
00417       cout << "Erreur = " << a << endl;
00418    } else {
00419       cout << "Ok" << endl;
00420    }
00421 
00422    cout << "Testing Stanford (DH) jacobian in tool frame : ";
00423    for(i = 1; i <= 6; i++) {
00424       for(int j = 1; j <= 6; j++) {
00425          infile >> Test(i,j);
00426       }
00427    }
00428    a = (Test-robot_DH.jacobian(dof)).MaximumAbsoluteValue();
00429    if(a > eps) {
00430       cout << "Erreur = " << a << endl;
00431    } else {
00432       cout << "Ok" << endl;
00433    }
00434 
00435    Test = Matrix(dof,1); Test = 0;
00436    cout << "Testing Stanford (DH) torque : ";
00437    for(i = 1; i <= dof; i++) {
00438       infile >> Test(i,1);
00439    }
00440    a = (Test-robot_DH.torque(qr, qd, qdd)).MaximumAbsoluteValue();
00441    if(a > eps) {
00442       cout << "Erreur = " << a << endl;
00443    } else {
00444       cout << "Ok" << endl;
00445    }
00446 
00447    cout << "Testing Stanford (DH) torque with load on link n: ";
00448    Fext(1)=10; Fext(2)=5; Fext(3)=7;
00449    Next(1)=11; Next(2)=22; Next(3)=33;
00450    for(i = 1; i <= dof; i++) {
00451       infile >> Test(i,1);
00452    }
00453    a = (Test-robot_DH.torque(qr, qd, qdd, Fext, Next)).MaximumAbsoluteValue();
00454    if(a > eps) {
00455       cout << "Erreur = " << a << endl;
00456    } else {
00457       cout << "Ok" << endl;
00458    }
00459 
00460    cout << "Testing Stanford (DH) inertia : ";
00461    Test = Matrix(6,6); Test = 0;
00462    for(i = 1; i <= 6; i++) {
00463       for(int j = 1; j <= 6; j++){
00464          infile >> Test(i,j);
00465       }
00466    }
00467    a = (Test-robot_DH.inertia(qr)).MaximumAbsoluteValue();
00468    if(a > eps) {
00469       cout << "Erreur = " << a << endl;
00470    } else {
00471       cout << "Ok" << endl;
00472    }
00473 
00474    Test = Matrix(4,4);
00475 
00476    // ATTENTION J'AI CHANGE LES PARAMETRES DH DANS PUMA560AKD.M, j'ai ecris a P. Corke
00477    // Puma 560 DH modified
00478    cout << "Testing Puma 560 (mDH) forward kinematic : ";
00479    for(i = 1; i <= 4; i++) {
00480       for(int j = 1; j <= 4; j++) {
00481          infile >> Test(i,j);
00482       }
00483    }
00484    initrob = Matrix(6,19);
00485    initrob << PUMA560_data_mDH;
00486    initrobm = Matrix(6,4);
00487    initrobm = 0.0;
00488    robot_mDH = mRobot(initrob, initrobm);
00489    dof = robot_mDH.get_dof();
00490    qr = ColumnVector(dof);
00491    qr = M_PI/4.0;
00492    robot_mDH.set_q(qr);
00493    a = (Test-robot_mDH.kine()).MaximumAbsoluteValue();
00494    if(a > eps) {
00495       cout << "Erreur = " << a << endl;
00496    } else {
00497       cout << "Ok" << endl;
00498    }
00499 
00500    // faut revoir jacobian pour dernier link
00501    cout << "Testing Puma 560 (mDH) jacobian in base frame : ";
00502    Test = Matrix(6,6);
00503    for(i = 1; i <= 6; i++) {
00504       for(int j = 1; j <= 6; j++) {
00505          infile >> Test(i,j);
00506       }
00507    }
00508    a = (Test-robot_mDH.jacobian()).MaximumAbsoluteValue();
00509    if(a > eps) {
00510       cout << "Erreur = " << a << endl;
00511    } else {
00512       cout << "Ok" << endl;
00513    }
00514 
00515    cout << "Testing Puma 560 (mDH) jacobian in tool frame : ";
00516    for(i = 1; i <= 6; i++) {
00517       for(int j = 1; j <= 6; j++) {
00518          infile >> Test(i,j);
00519       }
00520    }
00521    a = (Test-robot_mDH.jacobian(dof)).MaximumAbsoluteValue();
00522    if(a > eps) {
00523       cout << "Erreur = " << a << endl;
00524    } else {
00525       cout << "Ok" << endl;
00526    }
00527 
00528    initrobm = Matrix(6,4);
00529    initrobm = 0.0;
00530    robot_mDH = mRobot(initrob,initrobm);
00531 
00532    cout << "Testing Puma 560 (mDH) torque : ";
00533    Test = Matrix(dof,1);
00534    for(i = 1; i <= dof; i++) {
00535       infile >> Test(i,1);
00536    }
00537    a = (Test-robot_mDH.torque(qr, qd, qdd)).MaximumAbsoluteValue();
00538    if(a > eps) {
00539       cout << "Erreur = " << a << endl;
00540    } else {
00541       cout << "Ok" << endl;
00542    }
00543 
00544    cout << "Testing Puma 560 (mDH) inertia : ";
00545    Test = Matrix(6,6); Test = 0;
00546    for(i = 1; i <= 6; i++) {
00547       for(int j = 1; j <= 6; j++){
00548          infile >> Test(i,j);
00549       }
00550    }
00551    a = (Test-robot_mDH.inertia(qr)).MaximumAbsoluteValue();
00552    if(a > eps) {
00553       cout << "Erreur = " << a << endl;
00554    } else {
00555       cout << "Ok" << endl;
00556    }
00557 
00558    cout << "Testing Puma 560 (mDH) motor, torque : ";
00559    initrobm = Matrix(6,4);
00560    initrobm << PUMA560_motor;
00561    robot_mDH = mRobot(initrob,initrobm);
00562    Test = Matrix(dof,1);
00563    for(i = 1; i <= dof; i++) {
00564       infile >> Test(i,1);
00565    }
00566    a = (Test-robot_mDH.torque(qr, qd, qdd)).MaximumAbsoluteValue();
00567    if(a > eps) {
00568       cout << "Erreur = " << a << endl;
00569    } else {
00570       cout << "Ok" << endl;
00571    }
00572 
00573    cout << "Testing Puma 560 (mDH) motor, torque with load on link n: ";
00574    Fext(1)=10; Fext(2)=5; Fext(3)=7;
00575    Next(1)=11; Next(2)=22; Next(3)=33;
00576    for(i = 1; i <= dof; i++) {
00577       infile >> Test(i,1);
00578    }
00579    a = (Test-robot_mDH.torque(qr, qd, qdd, Fext, Next)).MaximumAbsoluteValue();
00580    if(a > eps) {
00581       cout << "Erreur = " << a << endl;
00582    } else {
00583       cout << "Ok" << endl;
00584    }
00585 
00586    return(0);
00587 }


kni
Author(s): Martin Günther
autogenerated on Mon Aug 14 2017 02:44:13