bench.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
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 Revision_history:
00032 
00033 2004/07/01: Ethan Tira-Thompson
00034     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00035 
00036 2004/07/02: Etienne Lachance
00037     -Added doxygen documentation.
00038 
00039 2004/08/13: Etienne Lachance
00040     -Modified robot initialisation matrix.
00041 
00042 2005/06/29: Richard Gourdeau
00043     -Added code for the Stewart platform by Samuel Belanger
00044 -------------------------------------------------------------------------------
00045 */
00046 
00054 
00055 static const char rcsid[] = "$Id: bench.cpp,v 1.20 2005/07/01 16:16:35 gourdeau Exp $";
00056 
00057 #include <time.h>
00058 
00059 #ifdef _WIN32          /* Windows 95/NT */
00060 #ifdef __GNUG__        /* Cygnus Gnu C++ for win32*/
00061 #include <windows.h>
00062 #define clock() GetTickCount() /* quick fix for bug in clock() */
00063 #endif
00064 #endif
00065 
00066 #include "robot.h"
00067 #include "stewart.h"
00068 
00069 #ifdef use_namespace
00070 using namespace ROBOOP;
00071 #endif
00072 
00073 #define NTRY 2000
00074 
00075 Real Stewart_Ini[] =
00076    {1.758, 2.8, -1.015, 0.225, 0.0, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
00077     1.6021, 3.07, -0.925, 0.1125, 0.1949, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
00078     -1.7580, 2.8, -1.015, -0.1125, 0.1949, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
00079     -1.6021, 3.07, -0.925, -0.225, 0.0, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
00080     0.0, 2.8, 2.03, -0.1125, -0.1949, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
00081     0.0, 3.07, 1.85, 0.1125, -0.1949, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
00082     0.0, 0.0, -0.114, 1.001, 0.59, 0.843, 10, 0.12, 0.04, 0.5, 0.5, 0.5, 1.5, 0.5, 0.005, 5.44, 0.443};
00083 
00084 Real Stewart_q[] =
00085    {0.2, 0.3, -0.4, 0.1, -1.4, 0.1};
00086 Real Stewart_qg[] =
00087    {0.25, 0.25, -0.45, 0.07, -1.7, 0.07};
00088 Real Stewart_l[] =
00089    {3.0508, 3.2324, 3.2997, 3.4560, 3.5797, 3.6935};
00090 Real Stewart_dq[] =
00091    {0.2, 0.3, -0.4, 0.1, -1.4, 0.1};
00092 Real Stewart_ddq[]=
00093    {-10.0, -10.0, -10, -10.0, -10, -10};
00094 Real Stewart_tddq[]=
00095    {0, 0, 0, 0, 0, 0};
00096 Real Comm[]=
00097    {1, 0, 0, -10, 0, 0};
00098 Real Tau[]=
00099    {126.219689, 789.968672, 0.708602, 79.122963, 81.806978, -31.61797};
00100 
00101 int stewartmain(void)
00102 {
00103    clock_t start, end;
00104    Matrix InitPlatt(7,17);
00105    Stewart platt_Stewart, platt2;
00106    Matrix _q(6,1), qg(6,1), _dq(6,1), _ddq(6,1), _l(6,1), To(6,1),
00107    V(6,1), L(6,1), tddq(6,1);
00108    int i;
00109 
00110    InitPlatt<<Stewart_Ini;
00111    _q << Stewart_q;
00112    qg << Stewart_qg;
00113    _l << Stewart_l;
00114    _dq << Stewart_dq;
00115    _ddq << Stewart_ddq;
00116    tddq << Stewart_tddq;
00117    V << Comm;
00118 
00119 
00120    platt_Stewart = Stewart(InitPlatt);
00121    // uncomment the next line to use the configuration file
00122    //platt_Stewart = Stewart("conf/stewart.conf","STEWART");
00123    platt_Stewart.set_q(_q);
00124    platt_Stewart.set_dq(_dq);
00125    platt_Stewart.set_ddq(_ddq);
00126 
00127    cout<<"===============================\n";
00128    cout<<"Benchmarks for Stewart platform\n";
00129    cout<<"===============================\n\n";
00130    cout<<"Inverse Kinematics :\n";
00131    start = clock();
00132    for(i =0; i<20000; i++)
00133       L = platt_Stewart.InvPosKine();
00134    end = clock();
00135    cout<<"Time : "<<((end - start)/(Real)CLOCKS_PER_SEC)*1000.0/20000<<"\nResult : "<<L.t()<<endl;
00136 
00137 
00138    cout<<"Forward Kinematics :\n";
00139    platt_Stewart.set_q(qg);
00140    start = clock();
00141    for(i =0; i<100; i++){
00142       L = platt_Stewart.ForwardKine(qg,_l);
00143       platt_Stewart.set_q(qg);}
00144    end = clock();
00145    cout<<"Time : "<<((end - start)/(Real)CLOCKS_PER_SEC)*1000.0/100<<"\nResult : "<<L.t()<<endl;
00146 
00147    platt_Stewart.set_q(_q);
00148 
00149    cout<<"Inverse Dynamics:\n";
00150    start = clock();
00151    for(i =0; i<100; i++)
00152       L = platt_Stewart.Torque();
00153    end = clock();
00154    cout<<"Time : "<<((end - start)/(Real)CLOCKS_PER_SEC)*1000.0/100<<"\nResult : "<<L.t()<<endl;
00155 
00156    platt_Stewart.set_ddq(tddq);
00157    To << Tau;
00158    cout<<"Forward Dynamics:\n";
00159    start = clock();
00160    for(i =0; i<100; i++)
00161       L = platt_Stewart.ForwardDyn(To);
00162    end = clock();
00163    cout<<"Time : "<<((end - start)/(Real)CLOCKS_PER_SEC)*1000.0/100<<"\nResult : "<<L.t()<<endl;
00164 
00165 
00166    double t = 0.01;
00167 
00168    L=platt_Stewart.ForwardDyn_AD(V,t);
00169 
00170    cout<<"Forward Dynamics with actuators:\nResult : "<<L.t()<<endl;
00171 
00172    return 0;
00173 }
00174 
00175 const Real PUMA560_data[] =
00176   {0, 0, 0, 0, M_PI/2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.35, 0, 0, 0, 0, 0, 0, 0, 
00177    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, 0, 0, 0, 0,
00178    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, 0, 0, 0, 0,
00179    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, 0, 0, 0, 0, 
00180    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, 0, 0, 0, 0,
00181    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, 0, 0, 0, 0};
00182 
00183 int main(void)
00184 {
00185    int i;
00186    clock_t start, end;
00187    Matrix initrob(6,23), thomo, temp2;
00188    ColumnVector q(6), qs(6), temp;
00189    Robot robot;
00190 
00191    initrob << PUMA560_data;
00192    robot = Robot(initrob);
00193    q = M_PI/6.0;
00194 
00195    q = M_PI/4.0;
00196    printf("=================================\n");
00197    printf("Benchmarks for serial 6 dof robot\n");
00198    printf("=================================\n\n");
00199 
00200    printf("Begin compute Forward Kinematics\n");
00201    start = clock();
00202    for (i = 1; i <= NTRY; i++) {
00203       robot.set_q(q);
00204       temp2 = robot.kine();
00205    }
00206    end = clock();
00207    printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
00208    printf("end \n");
00209 
00210    qs = 1.0;
00211    qs(1) = M_PI/16.0;
00212    robot.set_q(q);
00213    thomo = robot.kine();
00214    printf("Begin compute Inverse Kinematics\n");
00215    start = clock();
00216    for (i = 1; i <= NTRY; i++) {
00217       robot.set_q(qs);
00218       temp = robot.inv_kin(thomo);
00219    }
00220    end = clock();
00221    printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
00222    printf("end \n");
00223 
00224    printf("Begin compute Jacobian\n");
00225    start = clock();
00226    for (i = 1; i <= NTRY; i++) {
00227       robot.set_q(q);
00228       temp2 = robot.jacobian();
00229    }
00230    end = clock();
00231    printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
00232    printf("end \n");
00233 
00234    printf("Begin compute Torque\n");
00235    start = clock();
00236    for (i = 1; i <= NTRY; i++) {
00237       temp = robot.torque(q,q,q);
00238    }
00239    end = clock();
00240    printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
00241    printf("end \n");
00242 
00243    printf("Begin acceleration\n");
00244    start = clock();
00245    for (i = 1; i <= NTRY; i++) {
00246       temp = robot.acceleration(q,q,q);
00247    }
00248    end = clock();
00249    printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
00250    printf("end \n\n");
00251 
00252    stewartmain();
00253    return 0;
00254 }
00255 
00256 
00257 


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32