00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
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
00060 #ifdef __GNUG__
00061 #include <windows.h>
00062 #define clock() GetTickCount()
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
00122
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