ROBOOP, A Robotics Object Oriented Package in C++
demo.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, Professeur
23 Departement de genie electrique
24 Ecole Polytechnique de Montreal
25 C.P. 6079, Succ. Centre-Ville
26 Montreal, Quebec, H3C 3A7
27 
28 email: richard.gourdeau@polymtl.ca
29 
30 -------------------------------------------------------------------------------
31 Revision_history:
32 
33 2003/02/03: Etienne Lachance
34  -Added quaternions example in homogen_demo.
35  -Using function set_plot2d for gnuplot graphs.
36  -Declare vector with "dof" instead of hardcoded value.
37  -Changed RobotMotor to Robot.
38 
39 2003/29/04: Etienne Lachance
40  -Using Robot("conf/puma560_dh.conf", "PUMA560_DH") in kinematics functions.
41 
42 2004/07/01: Ethan Tira-Thompson
43  -Added support for newmat's use_namespace #define, using ROBOOP namespace
44 
45 2004/08/13: Etienne Lachance
46  -Modified robot initialisation matrix.
47 
48 2005/11/15 : Richard Gourdeau
49  - Fixed error on PUMA560 without motor dynamics
50 -------------------------------------------------------------------------------
51 
52 */
53 
61 #include "gnugraph.h"
62 #include "quaternion.h"
63 #include "robot.h"
64 #include "utils.h"
65 
66 #ifdef use_namespace
67 using namespace ROBOOP;
68 #endif
69 
70 void homogen_demo(void);
71 void kinematics_demo(void);
72 void dynamics_demo(void);
73 
74 
75 int main(void)
76 {
77  cout << "=====================================================\n";
78  cout << " ROBOOP -- A robotics object oriented package in C++ \n";;
79  cout << " DEMO program \n";
80  cout << "=====================================================\n";
81  cout << "\n";
82 
83  homogen_demo();
84  kinematics_demo();
85  dynamics_demo();
86 
87  return(0);
88 }
89 
90 
91 void homogen_demo(void)
92 {
93  ColumnVector p1(3), p2(3), p3(3);
94  Real ott[] = {1.0,2.0,3.0};
95  Real tto[] = {3.0,2.0,1.0};
96 
97  cout << "\n";
98  cout << "=====================================================\n";
99  cout << "Homogeneous transformations\n";
100  cout << "=====================================================\n";
101  cout << "\n";
102  cout << "Translation of [1;2;3]\n";
103  p1 << ott;
104  cout << setw(7) << setprecision(3) << trans(p1);
105  cout << "\n";
106  cout << "\n";
107  cout << "Rotation of pi/6 around axis X\n";
108  cout << setw(7) << setprecision(3) << rotx(M_PI/6);
109  cout << "\n";
110 
111  cout << "\n";
112  cout << "Rotation of pi/8 around axis Y\n";
113  cout << setw(7) << setprecision(3) << roty(M_PI/8);
114  cout << "\n";
115 
116  cout << "\n";
117  cout << "Rotation of pi/3 around axis Z\n";
118  cout << setw(7) << setprecision(3) << rotz(M_PI/3);
119  cout << "\n";
120 
121  cout << "\n";
122  cout << "Rotation of pi/3 around axis [1;2;3]\n";
123  cout << setw(7) << setprecision(3) << rotk(M_PI/3,p1);
124  cout << "\n";
125 
126  cout << "\n";
127  cout << "Rotation of pi/6 around line [1;2;3] -- [3;2;1]\n";
128  p2 << tto;
129  cout << setw(7) << setprecision(3) << rotd(M_PI/3,p1,p2);
130  cout << "\n";
131 
132  cout << "\n";
133  cout << "Roll Pitch Yaw Rotation [1;2;3]\n";
134  cout << setw(7) << setprecision(3) << rpy(p1);
135  cout << "\n";
136 
137  cout << "\n";
138  cout << "Euler ZXZ Rotation [3;2;1]\n";
139  cout << setw(7) << setprecision(3) << eulzxz(p2);
140  cout << "\n";
141 
142  cout << "\n";
143  cout << "Inverse of Rotation around axis\n";
144  cout << setw(7) << setprecision(3) << irotk(rotk(M_PI/3,p1));
145  cout << "\n";
146 
147  cout << "\n";
148  cout << "Inverse of Roll Pitch Yaw Rotation\n";
149  cout << setw(7) << setprecision(3) << irpy(rpy(p1));
150  cout << "\n";
151 
152  cout << "\n";
153  cout << "Inverse of Euler ZXZ Rotation\n";
154  cout << setw(7) << setprecision(3) << ieulzxz(eulzxz(p2));
155  cout << "\n";
156 
157  cout << "\n";
158  cout << "Cross product between [1;2;3] and [3;2;1]\n";
159  cout << setw(7) << setprecision(3) << CrossProduct(p1,p2);
160  cout << "\n";
161 
162  cout << "\n";
163  cout << "Rotation matrix from quaternion\n";
164  ColumnVector axis(3); axis(1)=axis(2)=0; axis(3)=1.0;
165  Quaternion q(M_PI/4, axis);
166  cout << setw(7) << setprecision(3) << q.R();
167  cout << "\n";
168 
169  cout << "\n";
170  cout << "Transformation matrix from quaternion\n";
171  cout << setw(7) << setprecision(3) << q.T();
172 
173  cout << "\n";
174  cout << "Quaternion from rotation matrix\n";
175  Quaternion qq(q.R());
176  cout << setw(7) << setprecision(3) << qq.s() << endl;
177  cout << setw(7) << setprecision(3) << qq.v() << endl;
178  cout << "\n";
179 
180  cout << "\n";
181  cout << "Quaternion from transformation matrix\n";
182  qq = Quaternion(q.T());
183  cout << setw(7) << setprecision(3) << qq.s() << endl;
184  cout << setw(7) << setprecision(3) << qq.v() << endl;
185 }
186 
187 const Real RR_data[] =
188  {0, 0, 0, 1.0, 0, 0, 0, 0, 2.0,-0.5, 0, 0, 0, 0, 0, 0.1666666, 0, 0.1666666, 0, 0, 0, 0, 0,
189  0, 0, 0, 1.0, 0, 0, 0, 0, 1.0,-0.5, 0, 0, 0, 0, 0, 0.0833333, 0, 0.0833333, 0, 0, 0, 0, 0};
190 const Real RR_data_mdh[] =
191  {0, 0, 0, 1.0, 0, 0, 0, 0, 2.0, 0.5, 0, 0, 0, 0, 0, 0.1666666, 0, 0.1666666, 0, 0, 0, 0, 0,
192  0, 0, 0, 1.0, 0, 0, 0, 0, 1.0, 0.5, 0, 0, 0, 0, 0, 0.0833333, 0, 0.0833333, 0, 0, 0, 0, 0};
193 const Real RR_data_mdh_min_para[] =
194  {0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0.0, 1.666666, 0, 0, 0, 0, 0,
195  0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0.5, 0, 0, -0.25, 0, 0, 0, 0.0, 0.3333333, 0, 0, 0, 0, 0};
196 
197 const Real RP_data[] =
198  {0, 0, 0, 0, -M_PI/2.0, 0, 0, 0, 2.0, 0, 0, 0.0, 1.0, 0, 0, 1.0, 0, 1.0, 0, 0, 0, 0, 0,
199  1, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0,-1.0, 0.0833333, 0, 0, 0.0833333, 0, 0.0833333, 0, 0, 0, 0, 0};
200 const Real PUMA560_data[] =
201  {0, 0, 0, 0, M_PI/2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.35, 0, 0, 0,
202  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,
203  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,
204  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,
205  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,
206  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};
207 const Real PUMA560_motor[] =
208  {200e-6, -62.6111, 1.48e-3, (.395 +.435)/2, /* using + and - directions average */
209  200e-6, 107.815, .817e-3, (.126 + .071)/2,
210  200e-6, -53.7063, 1.38e-3, (.132 + .105)/2,
211  33e-6, 76.0364, 71.2e-6, (11.2e-3 + 16.9e-3)/2,
212  33e-6, 71.923, 82.6e-6, (9.26e-3 + 14.5e-3)/2,
213  33e-6, 76.686, 36.7e-6, (3.96e-3 + 10.5e-3)/2};
214 
215 const Real STANFORD_data[] =
216  {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,
217  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,
218  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,
219  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,
220  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,
221  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};
222 
223 Robot robot;
224 Matrix K;
225 ColumnVector q0;
226 
227 ReturnMatrix xdot(Real t, const Matrix & x)
228 {
229  int ndof;
230  ColumnVector q, qp, qpp; /* position, velocities and accelerations */
231  ColumnVector tau, dx; /* torque and state space error */
232  Matrix xd;
233 
234  ndof = robot.get_dof(); /* get the # of dof */
235  q = x.SubMatrix(1,ndof,1,1); /* position, velocities */
236  qp = x.SubMatrix(ndof+1,2*ndof,1,1); /* from state vector */
237  qpp = ColumnVector(ndof);
238  qpp = 0.0; /* set the vector to zero */
239  // tau = robot.torque(q0,qpp,qpp); /* compute the gravity torque */
240  tau = ColumnVector(ndof);
241  tau = 0.0;
242 
243  dx = (q-q0) & qp; /* compute dx using vertical concatenation */
244  tau = tau - K*dx; /* feedback correction */
245  qpp = robot.acceleration(q, qp, tau); /* acceleration */
246  xd = qp & qpp; /* state vector derivative */
247 
248  xd.Release(); return xd;
249 }
250 
251 void kinematics_demo(void)
252 {
253  Matrix initrob(2,23), Tobj;
254  ColumnVector qs, qr;
255  int dof = 0;
256  int i;
257 
258  cout << "\n";
259  cout << "=====================================================\n";
260  cout << "Robot RR kinematics\n";
261  cout << "=====================================================\n";
262  initrob << RR_data;
263  robot = Robot(initrob);
264  dof = robot.get_dof();
265 
266  cout << "\n";
267  cout << "Robot D-H parameters\n";
268  cout << " type theta d a alpha\n";
269  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
270  cout << "\n";
271  cout << "Robot joints variables\n";
272  cout << setw(7) << setprecision(3) << robot.get_q();
273  cout << "\n";
274  cout << "Robot position\n";
275  cout << setw(7) << setprecision(3) << robot.kine();
276  cout << "\n";
277  qr = ColumnVector(dof);
278  qr = M_PI/4.0;
279  robot.set_q(qr);
280  cout << "Robot joints variables\n";
281  cout << setw(7) << setprecision(3) << robot.get_q();
282  cout << "\n";
283  cout << "Robot position\n";
284  cout << setw(7) << setprecision(3) << robot.kine();
285  cout << "\n";
286  cout << "Robot Jacobian w/r to base frame\n";
287  cout << setw(7) << setprecision(3) << robot.jacobian();
288  cout << "\n";
289  cout << "Robot Jacobian w/r to tool frame\n";
290  cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
291  cout << "\n";
292  for (i = 1; i <= qr.Nrows(); i++) {
293  cout << "Robot position partial derivative with respect to joint " << i << " \n";
294  cout << setw(7) << setprecision(3) << robot.dTdqi(i);
295  cout << "\n";
296  }
297  Tobj = robot.kine();
298  qs = ColumnVector(dof);
299  qs = M_PI/16.0;
300  robot.set_q(qs);
301  cout << "Robot inverse kinematics\n";
302  cout << " q start q final q real\n";
303  cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
304  cout << "\n";
305  cout << "\n";
306 
307  cout << "=====================================================\n";
308  cout << "Robot RP kinematics\n";
309  cout << "=====================================================\n";
310  initrob << RP_data;
311  robot = Robot(initrob);
312  dof = robot.get_dof();
313  cout << "\n";
314  cout << "Robot D-H parameters\n";
315  cout << " type theta d a alpha\n";
316  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
317  cout << "\n";
318  cout << "Robot joints variables\n";
319  cout << setw(7) << setprecision(3) << robot.get_q();
320  cout << "\n";
321  cout << "Robot position\n";
322  cout << setw(7) << setprecision(3) << robot.kine();
323  cout << "\n";
324  robot.set_q(M_PI/4.0,1);
325  robot.set_q(4.0,2);
326  cout << "Robot joints variables\n";
327  cout << setw(7) << setprecision(3) << robot.get_q();
328  cout << "\n";
329  cout << "Robot position\n";
330  cout << setw(7) << setprecision(3) << robot.kine();
331  cout << "\n";
332  cout << "Robot Jacobian w/r to base frame\n";
333  cout << setw(7) << setprecision(3) << robot.jacobian();
334  cout << "\n";
335  qr = robot.get_q();
336  cout << "Robot Jacobian w/r to tool frame\n";
337  cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
338  cout << "\n";
339  for (i = 1; i <= qr.Nrows(); i++) {
340  cout << "Robot position partial derivative with respect to joint " << i << " \n";
341  cout << setw(7) << setprecision(3) << robot.dTdqi(i);
342  cout << "\n";
343  }
344  Tobj = robot.kine();
345  robot.set_q(M_PI/16.0,1);
346  robot.set_q(1.0,2);
347  qs = robot.get_q();
348  cout << "Robot inverse kinematics\n";
349  cout << " q start q final q real\n";
350  cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
351  cout << "\n";
352  cout << "\n";
353 
354  cout << "=====================================================\n";
355  cout << "Robot PUMA560 kinematics\n";
356  cout << "=====================================================\n";
357  robot = Robot("conf/puma560_dh.conf", "PUMA560_DH");
358  dof = robot.get_dof();
359  cout << "\n";
360  cout << "Robot joints variables\n";
361  cout << setw(7) << setprecision(3) << robot.get_q();
362  cout << "\n";
363  cout << "Robot position\n";
364  cout << setw(7) << setprecision(3) << robot.kine();
365  cout << "\n";
366  qr = ColumnVector(dof);
367  qr = M_PI/4.0;
368  robot.set_q(qr);
369  cout << "Robot joints variables\n";
370  cout << setw(7) << setprecision(3) << robot.get_q();
371  cout << "\n";
372  cout << "Robot position\n";
373  cout << setw(7) << setprecision(3) << robot.kine();
374  cout << "\n";
375  cout << "Robot Jacobian w/r to base frame\n";
376  cout << setw(7) << setprecision(3) << robot.jacobian();
377  cout << "\n";
378  cout << "Robot Jacobian w/r to tool frame\n";
379  cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
380  cout << "\n";
381  for (i = 1; i <= qr.Nrows(); i++) {
382  cout << "Robot position partial derivative with respect to joint " << i << " \n";
383  cout << setw(7) << setprecision(3) << robot.dTdqi(i);
384  cout << "\n";
385  }
386  Tobj = robot.kine();
387  qs = ColumnVector(dof);
388  qs = 1.0;
389  qs(1) = M_PI/16.0;
390  robot.set_q(qs);
391  cout << "Robot inverse kinematics\n";
392  cout << " q start q final q real\n";
393  cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
394  cout << "\n";
395  cout << "\n";
396  cout << "=====================================================\n";
397  cout << "Robot STANFORD kinematics\n";
398  cout << "=====================================================\n";
399  initrob = Matrix(6,23);
400  initrob << STANFORD_data;
401  robot = Robot(initrob);
402  dof = robot.get_dof();
403  cout << "\n";
404  cout << "Robot D-H parameters\n";
405  cout << " type theta d a alpha\n";
406  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
407  cout << "\n";
408  cout << "Robot joints variables\n";
409  cout << setw(7) << setprecision(3) << robot.get_q();
410  cout << "\n";
411  cout << "Robot position\n";
412  cout << setw(7) << setprecision(3) << robot.kine();
413  cout << "\n";
414  qr = ColumnVector(dof);
415  qr = M_PI/4.0;
416  robot.set_q(qr);
417  cout << "Robot joints variables\n";
418  cout << setw(7) << setprecision(3) << robot.get_q();
419  cout << "\n";
420  cout << "Robot position\n";
421  cout << setw(7) << setprecision(3) << robot.kine();
422  cout << "\n";
423  cout << "Robot Jacobian w/r to base frame\n";
424  cout << setw(7) << setprecision(3) << robot.jacobian();
425  cout << "\n";
426  cout << "Robot Jacobian w/r to tool frame\n";
427  cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
428  cout << "\n";
429  for (i = 1; i <= qr.Nrows(); i++) {
430  cout << "Robot position partial derivative with respect to joint " << i << " \n";
431  cout << setw(7) << setprecision(3) << robot.dTdqi(i);
432  cout << "\n";
433  }
434  Tobj = robot.kine();
435  qs = ColumnVector(dof);
436  qs = 1.0;
437  qs(1) = M_PI/16.0;
438  robot.set_q(qs);
439  cout << "Robot inverse kinematics\n";
440  cout << " q start q final q real\n";
441  cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
442  cout << "\n";
443  cout << "\n";
444 }
445 
446 void dynamics_demo(void)
447 {
448  int nok, nbad, dof = 0;
449  Matrix initrob(2,23), Tobj, xout;
450  ColumnVector qs, qr;
451  RowVector tout;
452  int i;
453 
454  cout << "\n";
455  cout << "=====================================================\n";
456  cout << "Robot RR dynamics\n";
457  cout << "=====================================================\n";
458  initrob << RR_data;
459  robot = Robot(initrob);
460  dof = robot.get_dof();
461  cout << "\n";
462  cout << "Robot D-H parameters\n";
463  cout << " type theta d a alpha\n";
464  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
465  cout << "\n";
466  cout << "Robot D-H inertial parameters\n";
467  cout << " mass cx cy cz Ixx Ixy Ixz Iyy Iyz Izz\n";
468  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
469  cout << "\n";
470  cout << "Robot joints variables\n";
471  cout << setw(7) << setprecision(3) << robot.get_q();
472  cout << "\n";
473  cout << "Robot Inertia matrix\n";
474  cout << setw(7) << setprecision(3) << robot.inertia(robot.get_q());
475  cout << "\n";
476 
477  K = Matrix(dof,2*dof);
478  K = 0.0;
479  K(1,1) = K(2,2) = 25.0; /* K = [25I 7.071I ] */
480  K(1,3) = K(2,4) = 7.071;
481  cout << "Feedback gain matrix K\n";
482  cout << setw(7) << setprecision(3) << K;
483  cout << "\n";
484  q0 = ColumnVector(dof);
485  q0 = M_PI/4.0;
486  qs = ColumnVector(2*dof);
487  qs = 0.0;
488 
489  cout << " time ";
490  for(i = 1; i <= dof; i++)
491  cout <<"q" << i << " ";
492  for(i = 1; i <= dof; i++)
493  cout <<"qp" << i << " ";
494  cout << endl;
495 
496  /* Runge_Kutta4(xdot, qs, 0.0, 4.0, 160, tout, xout);
497  replaced by adaptive step size */
498  odeint(xdot, qs, 0.0, 4.0, 1e-4,0.1, 1e-12, nok, nbad, tout, xout, 0.05);
499  cout << setw(7) << setprecision(3) << (tout & xout).t();
500  cout << "\n";
501  cout << "\n";
502 
503 
504  set_plot2d("Robot joints position", "time (sec)", "q(i) (rad)", "q", DATAPOINTS,
505  tout, xout, 1, dof);
506 
507  /* uncomment the following two lines to obtain a
508  ps file of the graph */
509  /* plotposition.addcommand("set term post");
510  plotposition.addcommand("set output \"demo.ps\"");*/
511 
512  set_plot2d("Robot joints velocity", "time (sec)", "qp(i) (rad/s)", "qp", DATAPOINTS,
513  tout, xout, dof+1, 2*dof);
514 
515 
516  cout << "=====================================================\n";
517  cout << "Robot RP dynamics\n";
518  cout << "=====================================================\n";
519  initrob << RP_data;
520  robot = Robot(initrob);
521  dof = robot.get_dof();
522  cout << "\n";
523  cout << "Robot D-H parameters\n";
524  cout << " type theta d a alpha\n";
525  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
526  cout << "\n";
527  cout << "Robot D-H inertial parameters\n";
528  cout << " mass cx cy cz Ixx Ixy Ixz Iyy Iyz Izz\n";
529  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
530  cout << "\n";
531  cout << "Robot joints variables\n";
532  cout << setw(7) << setprecision(3) << robot.get_q();
533  cout << "\n";
534  cout << "Robot Inertia matrix\n";
535  cout << setw(7) << setprecision(3) << robot.inertia(robot.get_q());
536  cout << "\n";
537  cout << "\n";
538 
539  cout << "=====================================================\n";
540  cout << "Robot PUMA560 dynamics\n";
541  cout << "=====================================================\n";
542  initrob = Matrix(6,19);
543  initrob << PUMA560_data;
544  Matrix temp(6,4);
545  temp = 0; /* empty motor dynamics */
546  robot = Robot((initrob | temp));
547  dof = robot.get_dof();
548  cout << "\n";
549  cout << "Robot D-H parameters\n";
550  cout << " type theta d a alpha\n";
551  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
552  cout << "\n";
553  cout << "Robot D-H inertial parameters\n";
554  cout << " mass cx cy cz Ixx Ixy Ixz Iyy Iyz Izz\n";
555  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
556  cout << "\n";
557  cout << "Robot joints variables\n";
558  cout << setw(7) << setprecision(3) << robot.get_q();
559  cout << "\n";
560  cout << "Robot Inertia matrix\n";
561  cout << setw(8) << setprecision(4) << robot.inertia(robot.get_q());
562  cout << "\n";
563  qs = ColumnVector(dof);
564  qr = ColumnVector(dof);
565  qs =0.0;
566  qr =0.0;
567  cout << "Robot Torque\n";
568  cout << setw(8) << setprecision(4) << robot.torque(robot.get_q(),qs,qr);
569  cout << "\n";
570  cout << "Robot acceleration\n";
571  cout << setw(8) << setprecision(4) << robot.acceleration(robot.get_q(),qs,qr);
572  cout << "\n";
573 
574  cout << "\n";
575  cout << "=====================================================\n";
576  cout << "Robot STANFORD dynamics\n";
577  cout << "=====================================================\n";
578  initrob = Matrix(6,23);
579  initrob << STANFORD_data;
580  robot = Robot(initrob);
581  dof = robot.get_dof();
582  cout << "\n";
583  cout << "Robot D-H parameters\n";
584  cout << " type theta d a alpha\n";
585  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
586  cout << "\n";
587  cout << "Robot D-H inertial parameters\n";
588  cout << " mass cx cy cz Ixx Ixy Ixz Iyy Iyz Izz\n";
589  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
590  cout << "\n";
591  cout << "Robot joints variables\n";
592  cout << setw(7) << setprecision(3) << robot.get_q();
593  cout << "\n";
594  cout << "Robot Inertia matrix\n";
595  cout << setw(7) << setprecision(3) << robot.inertia(robot.get_q());
596  cout << "\n";
597  cout << "\n";
598 
599  cout << "=====================================================\n";
600  cout << "Robot PUMA560 with motors dynamics\n";
601  cout << "=====================================================\n";
602  initrob = Matrix(6,19);
603  initrob << PUMA560_data;
604  Matrix initrobm = Matrix(6,4);
605  initrobm << PUMA560_motor;
606  robot = Robot(initrob,initrobm);
607  dof =robot.get_dof();
608  cout << "\n";
609  cout << "Robot D-H parameters\n";
610  cout << " type theta d a alpha\n";
611  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
612  cout << "\n";
613  cout << "Robot D-H inertial parameters\n";
614  cout << " mass cx cy cz Ixx Ixy Ixz Iyy Iyz Izz\n";
615  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
616  cout << "\n";
617  cout << "Robot motors inertia, gear ratio, viscous and Coulomb friction coefficients\n";
618  cout << " Im Gr B Cf\n";
619  cout << setw(7) << setprecision(3) << initrobm;
620  cout << "\n";
621  cout << "Robot joints variables\n";
622  cout << setw(7) << setprecision(3) << robot.get_q();
623  cout << "\n";
624  cout << "Robot Inertia matrix\n";
625  cout << setw(8) << setprecision(4) << robot.inertia(robot.get_q());
626  cout << "\n";
627  qs = ColumnVector(dof);
628  qr = ColumnVector(dof);
629  qs =0.0;
630  qr =0.0;
631  robot.set_q(qs);
632  cout << "Robot Torque\n";
633  cout << setw(8) << setprecision(4) << robot.torque(robot.get_q(),qs,qr);
634  cout << "\n";
635  cout << "Robot acceleration\n";
636  cout << setw(8) << setprecision(4) << robot.acceleration(robot.get_q(),qs,qr);
637  cout << "\n";
638 }