bench.cpp
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 1996-2004 Richard Gourdeau
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 
20 Report problems and direct all questions to:
21 
22 Richard Gourdeau
23 Professeur Agrege
24 Departement de genie electrique
25 Ecole Polytechnique de Montreal
26 C.P. 6079, Succ. Centre-Ville
27 Montreal, Quebec, H3C 3A7
28 
29 email: richard.gourdeau@polymtl.ca
30 -------------------------------------------------------------------------------
31 Revision_history:
32 
33 2004/07/01: Ethan Tira-Thompson
34  -Added support for newmat's use_namespace #define, using ROBOOP namespace
35 
36 2004/07/02: Etienne Lachance
37  -Added doxygen documentation.
38 
39 2004/08/13: Etienne Lachance
40  -Modified robot initialisation matrix.
41 
42 2005/06/29: Richard Gourdeau
43  -Added code for the Stewart platform by Samuel Belanger
44 -------------------------------------------------------------------------------
45 */
46 
54 static const char rcsid[] = "$Id: bench.cpp,v 1.20 2005/07/01 16:16:35 gourdeau Exp $";
56 
57 #include <time.h>
58 
59 #ifdef _WIN32 /* Windows 95/NT */
60 #ifdef __GNUG__ /* Cygnus Gnu C++ for win32*/
61 #include <windows.h>
62 #define clock() GetTickCount() /* quick fix for bug in clock() */
63 #endif
64 #endif
65 
66 #include "robot.h"
67 #include "stewart.h"
68 
69 #ifdef use_namespace
70 using namespace ROBOOP;
71 #endif
72 
73 #define NTRY 2000
74 
76  {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,
77  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,
78  -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,
79  -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,
80  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,
81  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,
82  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};
83 
85  {0.2, 0.3, -0.4, 0.1, -1.4, 0.1};
87  {0.25, 0.25, -0.45, 0.07, -1.7, 0.07};
89  {3.0508, 3.2324, 3.2997, 3.4560, 3.5797, 3.6935};
91  {0.2, 0.3, -0.4, 0.1, -1.4, 0.1};
93  {-10.0, -10.0, -10, -10.0, -10, -10};
95  {0, 0, 0, 0, 0, 0};
97  {1, 0, 0, -10, 0, 0};
99  {126.219689, 789.968672, 0.708602, 79.122963, 81.806978, -31.61797};
100 
101 int stewartmain(void)
102 {
103  clock_t start, end;
104  Matrix InitPlatt(7,17);
105  Stewart platt_Stewart, platt2;
106  Matrix _q(6,1), qg(6,1), _dq(6,1), _ddq(6,1), _l(6,1), To(6,1),
107  V(6,1), L(6,1), tddq(6,1);
108  int i;
109 
110  InitPlatt<<Stewart_Ini;
111  _q << Stewart_q;
112  qg << Stewart_qg;
113  _l << Stewart_l;
114  _dq << Stewart_dq;
115  _ddq << Stewart_ddq;
116  tddq << Stewart_tddq;
117  V << Comm;
118 
119 
120  platt_Stewart = Stewart(InitPlatt);
121  // uncomment the next line to use the configuration file
122  //platt_Stewart = Stewart("conf/stewart.conf","STEWART");
123  platt_Stewart.set_q(_q);
124  platt_Stewart.set_dq(_dq);
125  platt_Stewart.set_ddq(_ddq);
126 
127  cout<<"===============================\n";
128  cout<<"Benchmarks for Stewart platform\n";
129  cout<<"===============================\n\n";
130  cout<<"Inverse Kinematics :\n";
131  start = clock();
132  for(i =0; i<20000; i++)
133  L = platt_Stewart.InvPosKine();
134  end = clock();
135  cout<<"Time : "<<((end - start)/(Real)CLOCKS_PER_SEC)*1000.0/20000<<"\nResult : "<<L.t()<<endl;
136 
137 
138  cout<<"Forward Kinematics :\n";
139  platt_Stewart.set_q(qg);
140  start = clock();
141  for(i =0; i<100; i++){
142  L = platt_Stewart.ForwardKine(qg,_l);
143  platt_Stewart.set_q(qg);}
144  end = clock();
145  cout<<"Time : "<<((end - start)/(Real)CLOCKS_PER_SEC)*1000.0/100<<"\nResult : "<<L.t()<<endl;
146 
147  platt_Stewart.set_q(_q);
148 
149  cout<<"Inverse Dynamics:\n";
150  start = clock();
151  for(i =0; i<100; i++)
152  L = platt_Stewart.Torque();
153  end = clock();
154  cout<<"Time : "<<((end - start)/(Real)CLOCKS_PER_SEC)*1000.0/100<<"\nResult : "<<L.t()<<endl;
155 
156  platt_Stewart.set_ddq(tddq);
157  To << Tau;
158  cout<<"Forward Dynamics:\n";
159  start = clock();
160  for(i =0; i<100; i++)
161  L = platt_Stewart.ForwardDyn(To);
162  end = clock();
163  cout<<"Time : "<<((end - start)/(Real)CLOCKS_PER_SEC)*1000.0/100<<"\nResult : "<<L.t()<<endl;
164 
165 
166  double t = 0.01;
167 
168  L=platt_Stewart.ForwardDyn_AD(V,t);
169 
170  cout<<"Forward Dynamics with actuators:\nResult : "<<L.t()<<endl;
171 
172  return 0;
173 }
174 
176  {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,
177  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,
178  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,
179  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,
180  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,
181  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};
182 
183 int main(void)
184 {
185  int i;
186  clock_t start, end;
187  Matrix initrob(6,23), thomo, temp2;
188  ColumnVector q(6), qs(6), temp;
189  Robot robot;
190 
191  initrob << PUMA560_data;
192  robot = Robot(initrob);
193  q = M_PI/6.0;
194 
195  q = M_PI/4.0;
196  printf("=================================\n");
197  printf("Benchmarks for serial 6 dof robot\n");
198  printf("=================================\n\n");
199 
200  printf("Begin compute Forward Kinematics\n");
201  start = clock();
202  for (i = 1; i <= NTRY; i++) {
203  robot.set_q(q);
204  temp2 = robot.kine();
205  }
206  end = clock();
207  printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
208  printf("end \n");
209 
210  qs = 1.0;
211  qs(1) = M_PI/16.0;
212  robot.set_q(q);
213  thomo = robot.kine();
214  printf("Begin compute Inverse Kinematics\n");
215  start = clock();
216  for (i = 1; i <= NTRY; i++) {
217  robot.set_q(qs);
218  temp = robot.inv_kin(thomo);
219  }
220  end = clock();
221  printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
222  printf("end \n");
223 
224  printf("Begin compute Jacobian\n");
225  start = clock();
226  for (i = 1; i <= NTRY; i++) {
227  robot.set_q(q);
228  temp2 = robot.jacobian();
229  }
230  end = clock();
231  printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
232  printf("end \n");
233 
234  printf("Begin compute Torque\n");
235  start = clock();
236  for (i = 1; i <= NTRY; i++) {
237  temp = robot.torque(q,q,q);
238  }
239  end = clock();
240  printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
241  printf("end \n");
242 
243  printf("Begin acceleration\n");
244  start = clock();
245  for (i = 1; i <= NTRY; i++) {
246  temp = robot.acceleration(q,q,q);
247  }
248  end = clock();
249  printf("MilliSeconds %6.2f\n", ((end - start) / (Real)CLOCKS_PER_SEC)*1000.0/NTRY);
250  printf("end \n\n");
251 
252  stewartmain();
253  return 0;
254 }
255 
256 
257 
Real Stewart_l[]
Definition: bench.cpp:88
void set_dq(const ColumnVector _dq)
Set the platform&#39;s speed.
Definition: stewart.cpp:1048
DH notation robot class.
Definition: robot.h:340
Real Stewart_Ini[]
Definition: bench.cpp:75
void set_ddq(const ColumnVector _ddq)
Set the platform&#39;s acceleration.
Definition: stewart.cpp:1066
virtual ReturnMatrix torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
Joint torque, without contact force, based on Recursive Newton-Euler formulation. ...
Definition: dynamics.cpp:135
Real Stewart_ddq[]
Definition: bench.cpp:92
ReturnMatrix ForwardDyn(const ColumnVector Torque, const Real Gravity=GRAVITY)
Return the acceleration vector of the platform (ddq)
Definition: stewart.cpp:1701
Robots class definitions.
static const char rcsid[]
RCS/CVS version.
Definition: bench.cpp:55
Real Stewart_qg[]
Definition: bench.cpp:86
double Real
Definition: include.h:307
Real Stewart_q[]
Definition: bench.cpp:84
const Real PUMA560_data[]
Definition: bench.cpp:175
ReturnMatrix acceleration(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau)
Joints acceleration without contact force.
Definition: dynamics.cpp:100
#define NTRY
Definition: bench.cpp:73
ReturnMatrix ForwardKine(const ColumnVector guess_q, const ColumnVector l_given, const Real tolerance=0.001)
Return the position vector of the platform (vector q)
Definition: stewart.cpp:1567
Real Stewart_dq[]
Definition: bench.cpp:90
void set_q(const ColumnVector &q)
Set the joint position vector.
Definition: robot.cpp:1137
TransposedMatrix t() const
Definition: newmat6.cpp:320
Real Tau[]
Definition: bench.cpp:98
void set_q(const ColumnVector _q)
Set the position of the platform.
Definition: stewart.cpp:1036
The usual rectangular matrix.
Definition: newmat.h:625
ReturnMatrix Torque(const Real Gravity=GRAVITY)
Return the torque vector of the platform.
Definition: stewart.cpp:1625
int stewartmain(void)
Definition: bench.cpp:101
void kine(Matrix &Rot, ColumnVector &pos) const
Direct kinematics at end effector.
Definition: kinemat.cpp:92
Real Stewart_tddq[]
Definition: bench.cpp:94
ReturnMatrix InvPosKine()
Return the lenght of the links in a vector.
Definition: stewart.cpp:1429
Stewart definitions.
Definition: stewart.h:143
Stewart class definitions.
int main(void)
Definition: bench.cpp:183
Real Comm[]
Definition: bench.cpp:96
Column vector.
Definition: newmat.h:1008
ReturnMatrix ForwardDyn_AD(const ColumnVector Command, const Real t)
Return the acceleration of the platform (Stewart platform mechanism dynamics including actuator dynam...
Definition: stewart.cpp:1792
ReturnMatrix inv_kin(const Matrix &Tobj, const int mj=0)
Overload inv_kin function.
Definition: invkine.cpp:218
virtual ReturnMatrix jacobian(const int ref=0) const
Jacobian of mobile links expressed at frame ref.
Definition: robot.h:357
Robot robot
Definition: demo.cpp:227


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16