ROBOOP, A Robotics Object Oriented Package in C++
robot.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  -Merge classes Link and mLink into Link.
35  -Added Robot_basic class, which is base class of Robot and mRobot.
36  -Use try/catch statement in dynamic memory allocation.
37  -Added member functions set_qp and set_qpp in Robot_basic.
38  -It is now possible to specify a min and max value of joint angle in
39  all the robot constructor.
40 
41 2003/05/18: Etienne Lachance
42  -Initialize the following vectors to zero in Ro_basic constructors: w, wp, vp, dw, dwp, dvp.
43  -Added vector z0 in robot_basic.
44 
45 2004/01/23: Etienne Lachance
46  -Added const in non reference argument for all functions.
47 
48 2004/03/01: Etienne Lachance
49  -Added non member function perturb_robot.
50 
51 2004/03/21: Etienne Lachance
52  -Added the following member functions: set_mc, set_r, set_I.
53 
54 2004/04/19: Etienne Lachane
55  -Added and fixed Robot::robotType_inv_kin(). First version of this member function
56  has been done by Vincent Drolet.
57 
58 2004/04/26: Etienne Lachance
59  -Added member functions Puma_DH, Puma_mDH, Rhino_DH, Rhino_mDH.
60 
61 2004/05/21: Etienne Lachance
62  -Added Doxygen comments.
63 
64 2004/07/01: Ethan Tira-Thompson
65  -Added support for newmat's use_namespace #define, using ROBOOP namespace
66 
67 2004/07/02: Etienne Lachance
68  -Modified Link constructor, Robot_basic constructor, Link::transform and
69  Link::get_q functions in order to added joint_offset.
70 
71 2004/07/13: Ethan Tira-Thompson
72  -if joint_offset isn't defined in the config file, it is set to theta
73 
74 2004/07/16: Ethan Tira-Thompson
75  -Added Link::immobile flag, accessor functions, and initialization code
76  -Added get_available_q* functions, modified set_q so it can take a list of
77  length `dof` or `get_available_dof()`.
78 
79 2004/07/17: Etienne Lachance
80  -Added immobile flag in Link constructor.
81 
82 2004/08/13: Etienne Lachance
83  -Initialisation of a robot with matrix can only be done for the following
84  two cases:
85  1) A n x 23 parameters (robot and motors parameters)
86  2) a n x 19 (robot parameters) and a n x 4 (motor parameters)
87  see Robot_basic constructors and Link constructor for more details.
88  -Replace select_*() and add_*() by select() and add().
89 
90 2004/09/18: Etienne Lachance
91  -Added angle_in_degre option in config file and Robot constructor
92 
93 2004/10/02: Etienne Lachance
94  -Added Schlling familly for analytic inverse kinematics.
95 
96 2004/12/10: Stephen Webb
97  - minor bug in constructor Robot_basic(const Robot_basic & x)
98 `
99 2005/11/06: Etienne Lachance
100  - No need to provide a copy constructor and the assignment operator
101  (operator=) for Link class. Instead we use the one provide by the
102  compiler.
103  -No need to provide an assignment operator for Robot, mRobot and
104  mRobot_min_para classes.
105 
106 2006/01/21: Etienne Lachance
107  -Functions Rhino_DH, Puma_DH, Schilling_DH, Rhino_mDH, Puma_mDH and
108  Schilling_mDH use const Robot_basic ref instead of const Robot_basic pointer.
109  -Prevent exceptions from leaving Robot_basic destructor.
110  -Catch exception by reference instead of by value.
111 -------------------------------------------------------------------------------
112 */
113 
119 #include <time.h>
120 #include "config.h"
121 #include "robot.h"
122 
123 #ifdef use_namespace
124 namespace ROBOOP {
125  using namespace NEWMAT;
126 #endif
127 
129 Real fourbyfourident[] = {1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0};
130 
132 Real threebythreeident[] ={1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0};
133 
146 Link::Link(const int jt, const Real it, const Real id, const Real ia, const Real ial,
147  const Real it_min, const Real it_max, const Real it_off, const Real mass,
148  const Real cmx, const Real cmy, const Real cmz, const Real ixx, const Real ixy,
149  const Real ixz, const Real iyy, const Real iyz, const Real izz, const Real iIm,
150  const Real iGr, const Real iB, const Real iCf, const bool dh,
151  const bool min_inertial_parameters, const bool immobile_):
152  R(3,3),
153  joint_type(jt),
154  theta(it),
155  d(id),
156  a(ia),
157  alpha(ial),
158  theta_min(it_min),
159  theta_max(it_max),
160  joint_offset(it_off),
161  DH(dh),
162  min_para(min_inertial_parameters),
163  p(3),
164  m(mass),
165  Im(iIm),
166  Gr(iGr),
167  B(iB),
168  Cf(iCf),
169  I(3,3),
170  immobile(immobile_)
171 {
172  if (joint_type == 0)
173  theta += joint_offset;
174  else
175  d += joint_offset;
176  Real ct, st, ca, sa;
177  ct = cos(theta);
178  st = sin(theta);
179  ca = cos(alpha);
180  sa = sin(alpha);
181 
182  qp = qpp = 0.0;
183 
184  if (DH)
185  {
186  R(1,1) = ct;
187  R(2,1) = st;
188  R(3,1) = 0.0;
189  R(1,2) = -ca*st;
190  R(2,2) = ca*ct;
191  R(3,2) = sa;
192  R(1,3) = sa*st;
193  R(2,3) = -sa*ct;
194  R(3,3) = ca;
195 
196  p(1) = a*ct;
197  p(2) = a*st;
198  p(3) = d;
199  }
200  else // modified DH notation
201  {
202  R(1,1) = ct;
203  R(2,1) = st*ca;
204  R(3,1) = st*sa;
205  R(1,2) = -st;
206  R(2,2) = ca*ct;
207  R(3,2) = sa*ct;
208  R(1,3) = 0;
209  R(2,3) = -sa;
210  R(3,3) = ca;
211 
212  p(1) = a;
213  p(2) = -sa*d;
214  p(3) = ca*d;
215  }
216 
217  if (min_para)
218  {
219  mc = ColumnVector(3);
220  mc(1) = cmx;
221  mc(2) = cmy; // mass * center of gravity
222  mc(3) = cmz;
223  }
224  else
225  {
226  r = ColumnVector(3);
227  r(1) = cmx; // center of gravity
228  r(2) = cmy;
229  r(3) = cmz;
230  }
231 
232  I(1,1) = ixx;
233  I(1,2) = I(2,1) = ixy;
234  I(1,3) = I(3,1) = ixz;
235  I(2,2) = iyy;
236  I(2,3) = I(3,2) = iyz;
237  I(3,3) = izz;
238 }
239 
240 void Link::transform(const Real q)
242 {
243  if (DH)
244  {
245  if(joint_type == 0)
246  {
247  Real ct, st, ca, sa;
248  theta = q + joint_offset;
249  ct = cos(theta);
250  st = sin(theta);
251  ca = R(3,3);
252  sa = R(3,2);
253 
254  R(1,1) = ct;
255  R(2,1) = st;
256  R(1,2) = -ca*st;
257  R(2,2) = ca*ct;
258  R(1,3) = sa*st;
259  R(2,3) = -sa*ct;
260  p(1) = a*ct;
261  p(2) = a*st;
262  }
263  else // prismatic joint
264  p(3) = d = q + joint_offset;
265  }
266  else // modified DH notation
267  {
268  Real ca, sa;
269  ca = R(3,3);
270  sa = -R(2,3);
271  if(joint_type == 0)
272  {
273  Real ct, st;
274  theta = q + joint_offset;
275  ct = cos(theta);
276  st = sin(theta);
277 
278  R(1,1) = ct;
279  R(2,1) = st*ca;
280  R(3,1) = st*sa;
281  R(1,2) = -st;
282  R(2,2) = ca*ct;
283  R(3,2) = sa*ct;
284  R(1,3) = 0;
285  }
286  else
287  {
288  d = q + joint_offset;
289  p(2) = -sa*d;
290  p(3) = ca*d;
291  }
292  }
293 }
294 
295 Real Link::get_q(void) const
301 {
302  if(joint_type == 0)
303  return (theta - joint_offset);
304  else
305  return (d - joint_offset);
306 }
307 
308 
309 void Link::set_r(const ColumnVector & r_)
311 {
312  if(r_.Nrows() == 3)
313  r = r_;
314  else
315  cerr << "Link::set_r: wrong size in input vector." << endl;
316 }
317 
318 void Link::set_mc(const ColumnVector & mc_)
320 {
321  if(mc_.Nrows() == 3)
322  mc = mc_;
323  else
324  cerr << "Link::set_mc: wrong size in input vector." << endl;
325 }
326 
331 void Link::set_I(const Matrix & I_)
332 {
333  if( (I_.Nrows() == 3) && (I_.Ncols() == 3) )
334  I = I_;
335  else
336  cerr << "Link::set_r: wrong size in input vector." << endl;
337 }
338 
339 Robot_basic::Robot_basic(const Matrix & dhinit, const bool dh_parameter,
340  const bool min_inertial_para)
350 {
351  int ndof=0, i;
352 
353  gravity = ColumnVector(3);
354  gravity = 0.0;
355  gravity(3) = GRAVITY;
356  z0 = ColumnVector(3);
357  z0(1) = z0(2) = 0.0; z0(3) = 1.0;
358  fix = 0;
359  for(i = 1; i <= dhinit.Nrows(); i++)
360  if(dhinit(i,1) == 2)
361  {
362  if (i == dhinit.Nrows())
363  fix = 1;
364  else
365  error("Fix link can only be on the last one");
366  }
367  else
368  ndof++;
369 
370  if(ndof < 1)
371  error("Number of degree of freedom must be greater or equal to 1");
372 
373  dof = ndof;
374 
375  try
376  {
377  links = new Link[dof+fix];
378  links = links-1;
379  w = new ColumnVector[dof+1];
380  wp = new ColumnVector[dof+1];
381  vp = new ColumnVector[dof+fix+1];
382  a = new ColumnVector[dof+1];
383  f = new ColumnVector[dof+1];
384  f_nv = new ColumnVector[dof+1];
385  n = new ColumnVector[dof+1];
386  n_nv = new ColumnVector[dof+1];
387  F = new ColumnVector[dof+1];
388  N = new ColumnVector[dof+1];
389  p = new ColumnVector[dof+fix+1];
390  pp = new ColumnVector[dof+fix+1];
391  dw = new ColumnVector[dof+1];
392  dwp = new ColumnVector[dof+1];
393  dvp = new ColumnVector[dof+1];
394  da = new ColumnVector[dof+1];
395  df = new ColumnVector[dof+1];
396  dn = new ColumnVector[dof+1];
397  dF = new ColumnVector[dof+1];
398  dN = new ColumnVector[dof+1];
399  dp = new ColumnVector[dof+1];
400  R = new Matrix[dof+fix+1];
401  }
402  catch(bad_alloc & e)
403  {
404  cerr << "Robot_basic constructor : new ran out of memory" << endl;
405  cleanUpPointers();
406  throw;
407  }
408 
409  for(i = 0; i <= dof; i++)
410  {
411  w[i] = ColumnVector(3);
412  w[i] = 0.0;
413  wp[i] = ColumnVector(3);
414  wp[i] = 0.0;
415  vp[i] = ColumnVector(3);
416  dw[i] = ColumnVector(3);
417  dw[i] = 0.0;
418  dwp[i] = ColumnVector(3);
419  dwp[i] = 0.0;
420  dvp[i] = ColumnVector(3);
421  dvp[i] = 0.0;
422  }
423  for(i = 0; i <= dof+fix; i++)
424  {
425  R[i] = Matrix(3,3);
426  R[i] << threebythreeident;
427  p[i] = ColumnVector(3);
428  p[i] = 0.0;
429  pp[i] = p[i];
430  }
431 
432  if(dhinit.Ncols() == 23)
433  {
434  for(i = 1; i <= dof+fix; i++)
435  links[i] = Link((int) dhinit(i,1), dhinit(i,2), dhinit(i,3),
436  dhinit(i,4), dhinit(i,5), dhinit(i,6), dhinit(i,7),
437  dhinit(i,8), dhinit(i,9), dhinit(i,10), dhinit(i,11),
438  dhinit(i,12), dhinit(i,13), dhinit(i,14), dhinit(i,15),
439  dhinit(i,16), dhinit(i,17), dhinit(i,18), dhinit(i,19),
440  dhinit(i,20), dhinit(i,21), dhinit(i,22),
441  dh_parameter, min_inertial_para, dhinit(i,23));
442  }
443  else
444  error("Initialisation Matrix does not have 23 columns.");
445 }
446 
447 Robot_basic::Robot_basic(const Matrix & initrobot, const Matrix & initmotor,
448  const bool dh_parameter, const bool min_inertial_para)
459 {
460  int ndof=0, i;
461 
462  gravity = ColumnVector(3);
463  gravity = 0.0;
464  gravity(3) = GRAVITY;
465  z0 = ColumnVector(3);
466  z0(1) = z0(2) = 0.0; z0(3) = 1.0;
467  fix = 0;
468 
469  for(i = 1; i <= initrobot.Nrows(); i++)
470  if(initrobot(i,1) == 2)
471  {
472  if (i == initrobot.Nrows())
473  fix = 1;
474  else
475  error("Fix link can only be on the last one");
476  }
477  else
478  ndof++;
479 
480  if(ndof < 1)
481  error("Number of degree of freedom must be greater or equal to 1");
482  dof = ndof;
483 
484  try
485  {
486  links = new Link[dof+fix];
487  links = links-1;
488  w = new ColumnVector[dof+1];
489  wp = new ColumnVector[dof+1];
490  vp = new ColumnVector[dof+fix+1];
491  a = new ColumnVector[dof+1];
492  f = new ColumnVector[dof+1];
493  f_nv = new ColumnVector[dof+1];
494  n = new ColumnVector[dof+1];
495  n_nv = new ColumnVector[dof+1];
496  F = new ColumnVector[dof+1];
497  N = new ColumnVector[dof+1];
498  p = new ColumnVector[dof+fix+1];
499  pp = new ColumnVector[dof+fix+1];
500  dw = new ColumnVector[dof+1];
501  dwp = new ColumnVector[dof+1];
502  dvp = new ColumnVector[dof+1];
503  da = new ColumnVector[dof+1];
504  df = new ColumnVector[dof+1];
505  dn = new ColumnVector[dof+1];
506  dF = new ColumnVector[dof+1];
507  dN = new ColumnVector[dof+1];
508  dp = new ColumnVector[dof+1];
509  R = new Matrix[dof+fix+1];
510  }
511  catch(bad_alloc & e)
512  {
513  cerr << "Robot_basic constructor : new ran out of memory" << endl;
514  cleanUpPointers();
515  throw;
516  }
517 
518  for(i = 0; i <= dof; i++)
519  {
520  w[i] = ColumnVector(3);
521  w[i] = 0.0;
522  wp[i] = ColumnVector(3);
523  wp[i] = 0.0;
524  vp[i] = ColumnVector(3);
525  dw[i] = ColumnVector(3);
526  dw[i] = 0.0;
527  dwp[i] = ColumnVector(3);
528  dwp[i] = 0.0;
529  dvp[i] = ColumnVector(3);
530  dvp[i] = 0.0;
531  }
532  for(i = 0; i <= dof+fix; i++)
533  {
534  R[i] = Matrix(3,3);
535  R[i] << threebythreeident;
536  p[i] = ColumnVector(3);
537  p[i] = 0.0;
538  pp[i] = p[i];
539  }
540 
541  if ( initrobot.Nrows() == initmotor.Nrows() )
542  {
543  if(initmotor.Ncols() == 4)
544  {
545  if(initrobot.Ncols() == 19)
546  {
547  for(i = 1; i <= dof+fix; i++)
548  links[i] = Link((int) initrobot(i,1), initrobot(i,2), initrobot(i,3),
549  initrobot(i,4), initrobot(i,5), initrobot(i,6),
550  initrobot(i,7), initrobot(i,8), initrobot(i,9),
551  initrobot(i,10),initrobot(i,11), initrobot(i,12),
552  initrobot(i,13),initrobot(i,14), initrobot(i,15),
553  initrobot(i,16),initrobot(i,17), initrobot(i,18),
554  initmotor(i,1), initmotor(i,2), initmotor(i,3),
555  initmotor(i,4), dh_parameter, min_inertial_para,
556  initrobot(i,19));
557  }
558  else
559  error("Initialisation robot Matrix does not have 19 columns.");
560  }
561  else
562  error("Initialisation robot motor Matrix does not have 4 columns.");
563 
564  }
565  else
566  error("Initialisation robot and motor matrix does not have same numbers of Rows.");
567 }
568 
569 Robot_basic::Robot_basic(const int ndof, const bool dh_parameter,
570  const bool min_inertial_para)
580 {
581  int i = 0;
582  gravity = ColumnVector(3);
583  gravity = 0.0;
584  gravity(3) = GRAVITY;
585  z0 = ColumnVector(3);
586  z0(1) = z0(2) = 0.0; z0(3) = 1.0;
587  dof = ndof;
588  fix = 0;
589 
590  try
591  {
592  links = new Link[dof];
593  links = links-1;
594  w = new ColumnVector[dof+1];
595  wp = new ColumnVector[dof+1];
596  vp = new ColumnVector[dof+1];
597  a = new ColumnVector[dof+1];
598  f = new ColumnVector[dof+1];
599  f_nv = new ColumnVector[dof+1];
600  n = new ColumnVector[dof+1];
601  n_nv = new ColumnVector[dof+1];
602  F = new ColumnVector[dof+1];
603  N = new ColumnVector[dof+1];
604  p = new ColumnVector[dof+1];
605  pp = new ColumnVector[dof+fix+1];
606  dw = new ColumnVector[dof+1];
607  dwp = new ColumnVector[dof+1];
608  dvp = new ColumnVector[dof+1];
609  da = new ColumnVector[dof+1];
610  df = new ColumnVector[dof+1];
611  dn = new ColumnVector[dof+1];
612  dF = new ColumnVector[dof+1];
613  dN = new ColumnVector[dof+1];
614  dp = new ColumnVector[dof+1];
615  R = new Matrix[dof+1];
616  }
617  catch(bad_alloc & e)
618  {
619  cerr << "Robot_basic constructor : new ran out of memory" << endl;
620  cleanUpPointers();
621  }
622 
623  for(i = 0; i <= dof; i++)
624  {
625  w[i] = ColumnVector(3);
626  w[i] = 0.0;
627  wp[i] = ColumnVector(3);
628  wp[i] = 0.0;
629  vp[i] = ColumnVector(3);
630  dw[i] = ColumnVector(3);
631  dw[i] = 0.0;
632  dwp[i] = ColumnVector(3);
633  dwp[i] = 0.0;
634  dvp[i] = ColumnVector(3);
635  dvp[i] = 0.0;
636  }
637  for(i = 0; i <= dof+fix; i++)
638  {
639  R[i] = Matrix(3,3);
640  R[i] << threebythreeident;
641  p[i] = ColumnVector(3);
642  p[i] = 0.0;
643  pp[i] = p[i];
644  }
645 }
646 
647 Robot_basic::Robot_basic(const Robot_basic & x)
649 {
650  int i = 0;
651  dof = x.dof;
652  fix = x.fix;
653  gravity = x.gravity;
654  z0 = x.z0;
655 
656  try
657  {
658  links = new Link[dof+fix];
659  links = links-1;
660  w = new ColumnVector[dof+1];
661  wp = new ColumnVector[dof+1];
662  vp = new ColumnVector[dof+1];
663  a = new ColumnVector[dof+1];
664  f = new ColumnVector[dof+1];
665  f_nv = new ColumnVector[dof+1];
666  n = new ColumnVector[dof+1];
667  n_nv = new ColumnVector[dof+1];
668  F = new ColumnVector[dof+1];
669  N = new ColumnVector[dof+1];
670  p = new ColumnVector[dof+fix+1];
671  pp = new ColumnVector[dof+fix+1];
672  dw = new ColumnVector[dof+1];
673  dwp = new ColumnVector[dof+1];
674  dvp = new ColumnVector[dof+1];
675  da = new ColumnVector[dof+1];
676  df = new ColumnVector[dof+1];
677  dn = new ColumnVector[dof+1];
678  dF = new ColumnVector[dof+1];
679  dN = new ColumnVector[dof+1];
680  dp = new ColumnVector[dof+1];
681  R = new Matrix[dof+fix+1];
682  }
683  catch(bad_alloc & e)
684  {
685  cerr << "Robot_basic constructor : new ran out of memory" << endl;
686  cleanUpPointers();
687  }
688 
689  for(i = 0; i <= dof; i++)
690  {
691  w[i] = ColumnVector(3);
692  w[i] = 0.0;
693  wp[i] = ColumnVector(3);
694  wp[i] = 0.0;
695  vp[i] = ColumnVector(3);
696  dw[i] = ColumnVector(3);
697  dw[i] = 0.0;
698  dwp[i] = ColumnVector(3);
699  dwp[i] = 0.0;
700  dvp[i] = ColumnVector(3);
701  dvp[i] = 0.0;
702  }
703  for(i = 0; i <= dof+fix; i++)
704  {
705  R[i] = Matrix(3,3);
706  R[i] << threebythreeident;
707  p[i] = ColumnVector(3);
708  p[i] = 0.0;
709  pp[i] = p[i];
710  }
711 
712  for(i = 1; i <= dof+fix; i++)
713  links[i] = x.links[i];
714 }
715 
716 Robot_basic::Robot_basic(const string & filename, const string & robotName,
717  const bool dh_parameter, const bool min_inertial_para)
727 {
728  int i = 0;
729  gravity = ColumnVector(3);
730  gravity = 0.0;
731  gravity(3) = GRAVITY;
732  z0 = ColumnVector(3);
733  z0(1) = z0(2) = 0.0; z0(3) = 1.0;
734 
735  Config robData(true);
736  ifstream inconffile(filename.c_str(), std::ios::in);
737  if (robData.read_conf(inconffile))
738  {
739  error("Robot_basic::Robot_basic: unable to read input config file.");
740  }
741 
742  bool motor = false, angle_in_degree = false;
743 
744  if(robData.select(robotName, "dof", dof))
745  {
746  if(dof < 1)
747  error("Robot_basic::Robot_basic: dof is less then one.");
748  }
749  else
750  error("Robot_basic::Robot_basic: error in extracting dof from conf file.");
751 
752 
753  robData.select(robotName, "Fix", fix);
754  robData.select(robotName, "Motor", motor);
755  robData.select(robotName, "angle_in_degree", angle_in_degree);
756 
757  try
758  {
759  links = new Link[dof+fix];
760  links = links-1;
761  w = new ColumnVector[dof+1];
762  wp = new ColumnVector[dof+1];
763  vp = new ColumnVector[dof+fix+1];
764  a = new ColumnVector[dof+1];
765  f = new ColumnVector[dof+1];
766  f_nv = new ColumnVector[dof+1];
767  n = new ColumnVector[dof+1];
768  n_nv = new ColumnVector[dof+1];
769  F = new ColumnVector[dof+1];
770  N = new ColumnVector[dof+1];
771  p = new ColumnVector[dof+fix+1];
772  pp = new ColumnVector[dof+fix+1];
773  dw = new ColumnVector[dof+1];
774  dwp = new ColumnVector[dof+1];
775  dvp = new ColumnVector[dof+1];
776  da = new ColumnVector[dof+1];
777  df = new ColumnVector[dof+1];
778  dn = new ColumnVector[dof+1];
779  dF = new ColumnVector[dof+1];
780  dN = new ColumnVector[dof+1];
781  dp = new ColumnVector[dof+1];
782  R = new Matrix[dof+fix+1];
783  }
784  catch(bad_alloc & e)
785  {
786  cerr << "Robot_basic constructor : new ran out of memory" << endl;
787  cleanUpPointers();
788  }
789 
790  for(i = 0; i <= dof; i++)
791  {
792  w[i] = ColumnVector(3);
793  w[i] = 0.0;
794  wp[i] = ColumnVector(3);
795  wp[i] = 0.0;
796  vp[i] = ColumnVector(3);
797  dw[i] = ColumnVector(3);
798  dw[i] = 0.0;
799  dwp[i] = ColumnVector(3);
800  dwp[i] = 0.0;
801  dvp[i] = ColumnVector(3);
802  dvp[i] = 0.0;
803  }
804  for(i = 0; i <= dof+fix; i++)
805  {
806  R[i] = Matrix(3,3);
807  R[i] << threebythreeident;
808  p[i] = ColumnVector(3);
809  p[i] = 0.0;
810  pp[i] = p[i];
811  }
812 
813  for(int j = 1; j <= dof+fix; j++)
814  {
815  int joint_type =0;
816  double theta=0, d=0, a=0, alpha=0, theta_min=0, theta_max=0, joint_offset=0,
817  m=0, cx=0, cy=0, cz=0, Ixx=0, Ixy=0, Ixz=0, Iyy=0, Iyz=0, Izz=0,
818  Im=0, Gr=0, B=0, Cf=0;
819  bool immobile=false;
820 
821  string robotName_link;
822  ostringstream ostr;
823  ostr << robotName << "_LINK" << j;
824  robotName_link = ostr.str();
825 
826  robData.select(robotName_link, "joint_type", joint_type);
827  robData.select(robotName_link, "theta", theta);
828  robData.select(robotName_link, "d", d);
829  robData.select(robotName_link, "a", a);
830  robData.select(robotName_link, "alpha", alpha);
831  robData.select(robotName_link, "theta_max", theta_max);
832  robData.select(robotName_link, "theta_min", theta_min);
833  robData.select(robotName_link, "joint_offset", joint_offset);
834  robData.select(robotName_link, "m", m);
835  robData.select(robotName_link, "cx", cx);
836  robData.select(robotName_link, "cy", cy);
837  robData.select(robotName_link, "cz", cz);
838  robData.select(robotName_link, "Ixx", Ixx);
839  robData.select(robotName_link, "Ixy", Ixy);
840  robData.select(robotName_link, "Ixz", Ixz);
841  robData.select(robotName_link, "Iyy", Iyy);
842  robData.select(robotName_link, "Iyz", Iyz);
843  robData.select(robotName_link, "Izz", Izz);
844  robData.select(robotName_link,"immobile", immobile);
845 
846  if(angle_in_degree)
847  {
848  theta = deg2rad(theta);
849  theta_max = deg2rad(theta_max);
850  theta_min = deg2rad(theta_min);
851  alpha = deg2rad(alpha);
852  joint_offset = deg2rad(joint_offset);
853  }
854 
855  if(motor)
856  {
857  robData.select(robotName_link, "Im", Im);
858  robData.select(robotName_link, "Gr", Gr);
859  robData.select(robotName_link, "B", B);
860  robData.select(robotName_link, "Cf", Cf);
861  }
862 
863  links[j] = Link(joint_type, theta, d, a, alpha, theta_min, theta_max,
864  joint_offset, m, cx, cy, cz, Ixx, Ixy, Ixz, Iyy, Iyz,
865  Izz, Im, Gr, B, Cf, dh_parameter, min_inertial_para);
866  links[j].set_immobile(immobile);
867  }
868 
869  if(fix)
870  links[dof+fix] = Link(2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
871  0, 0, 0, 0, 0, 0, 0, 0, 0, dh_parameter, min_inertial_para);
872 
873 }
874 
875 Robot_basic::~Robot_basic()
876 
881 {
882  cleanUpPointers();
883 }
884 
885 Robot_basic & Robot_basic::operator=(const Robot_basic & x)
887 {
888  if (this != &x)
889  {
890  int i = 0;
891  if ( (dof != x.dof) || (fix != x.fix) )
892  {
893  links = links+1;
894  delete []links;
895  delete []R;
896  delete []dp;
897  delete []dN;
898  delete []dF;
899  delete []dn;
900  delete []df;
901  delete []da;
902  delete []dvp;
903  delete []dwp;
904  delete []dw;
905  delete []pp;
906  delete []p;
907  delete []N;
908  delete []F;
909  delete []n_nv;
910  delete []n;
911  delete []f_nv;
912  delete []f;
913  delete []a;
914  delete []vp;
915  delete []wp;
916  delete []w;
917  dof = x.dof;
918  fix = x.fix;
919  gravity = x.gravity;
920  z0 = x.z0;
921 
922  try
923  {
924  links = new Link[dof+fix];
925  links = links-1;
926  w = new ColumnVector[dof+1];
927  wp = new ColumnVector[dof+1];
928  vp = new ColumnVector[dof+fix+1];
929  a = new ColumnVector[dof+1];
930  f = new ColumnVector[dof+1];
931  f_nv = new ColumnVector[dof+1];
932  n = new ColumnVector[dof+1];
933  n_nv = new ColumnVector[dof+1];
934  F = new ColumnVector[dof+1];
935  N = new ColumnVector[dof+1];
936  p = new ColumnVector[dof+fix+1];
937  pp = new ColumnVector[dof+fix+1];
938  dw = new ColumnVector[dof+1];
939  dwp = new ColumnVector[dof+1];
940  dvp = new ColumnVector[dof+1];
941  da = new ColumnVector[dof+1];
942  df = new ColumnVector[dof+1];
943  dn = new ColumnVector[dof+1];
944  dF = new ColumnVector[dof+1];
945  dN = new ColumnVector[dof+1];
946  dp = new ColumnVector[dof+1];
947  R = new Matrix[dof+fix+1];
948  }
949  catch(bad_alloc & e)
950  {
951  cerr << "Robot_basic::operator= : new ran out of memory" << endl;
952  exit(1);
953  }
954  }
955  for(i = 0; i <= dof; i++)
956  {
957  w[i] = ColumnVector(3);
958  w[i] = 0.0;
959  wp[i] = ColumnVector(3);
960  wp[i] = 0.0;
961  vp[i] = ColumnVector(3);
962  dw[i] = ColumnVector(3);
963  dw[i] = 0.0;
964  dwp[i] = ColumnVector(3);
965  dwp[i] = 0.0;
966  dvp[i] = ColumnVector(3);
967  dvp[i] = 0.0;
968  }
969  for(i = 0; i <= dof+fix; i++)
970  {
971  R[i] = Matrix(3,3);
972  R[i] << threebythreeident;
973  p[i] = ColumnVector(3);
974  p[i] = 0.0;
975  pp[i] = p[i];
976  }
977  for(i = 1; i <= dof+fix; i++)
978  links[i] = x.links[i];
979  }
980  return *this;
981 }
982 
983 void Robot_basic::error(const string & msg1) const
985 {
986  cerr << endl << "Robot error: " << msg1.c_str() << endl;
987  // exit(1);
988 }
989 
990 int Robot_basic::get_available_dof(const int endlink)const
992 {
993  int ans=0;
994  for(int i=1; i<=endlink; i++)
995  if(!links[i].immobile)
996  ans++;
997  return ans;
998 }
999 
1000 ReturnMatrix Robot_basic::get_q(void)const
1002 {
1003  ColumnVector q(dof);
1004 
1005  for(int i = 1; i <= dof; i++)
1006  q(i) = links[i].get_q();
1007  q.Release(); return q;
1008 }
1009 
1010 ReturnMatrix Robot_basic::get_qp(void)const
1012 {
1013  ColumnVector qp(dof);
1014 
1015  for(int i = 1; i <= dof; i++)
1016  qp(i) = links[i].qp;
1017  qp.Release(); return qp;
1018 }
1019 
1020 ReturnMatrix Robot_basic::get_qpp(void)const
1022 {
1023  ColumnVector qpp(dof);
1024 
1025  for(int i = 1; i <= dof; i++)
1026  qpp(i) = links[i].qpp;
1027  qpp.Release(); return qpp;
1028 }
1029 
1030 ReturnMatrix Robot_basic::get_available_q(const int endlink)const
1032 {
1033  ColumnVector q(get_available_dof(endlink));
1034 
1035  int j=1;
1036  for(int i = 1; i <= endlink; i++)
1037  if(!links[i].immobile)
1038  q(j++) = links[i].get_q();
1039  q.Release(); return q;
1040 }
1041 
1042 ReturnMatrix Robot_basic::get_available_qp(const int endlink)const
1044 {
1045  ColumnVector qp(get_available_dof(endlink));
1046 
1047  int j=1;
1048  for(int i = 1; i <= endlink; i++)
1049  if(!links[i].immobile)
1050  qp(j++) = links[i].qp;
1051  qp.Release(); return qp;
1052 }
1053 
1054 ReturnMatrix Robot_basic::get_available_qpp(const int endlink)const
1056 {
1057  ColumnVector qpp(get_available_dof(endlink));
1058 
1059  int j=1;
1060  for(int i = 1; i <= endlink; i++)
1061  if(!links[i].immobile)
1062  qpp(j) = links[i].qpp;
1063  qpp.Release(); return qpp;
1064 }
1065 
1066 void Robot_basic::set_q(const Matrix & q)
1074 {
1075  int adof=get_available_dof();
1076  if(q.Nrows() == dof && q.Ncols() == 1) {
1077  for(int i = 1; i <= dof; i++)
1078  {
1079  links[i].transform(q(i,1));
1080  if(links[1].DH)
1081  {
1082  p[i](1) = links[i].get_a();
1083  p[i](2) = links[i].get_d() * links[i].R(3,2);
1084  p[i](3) = links[i].get_d() * links[i].R(3,3);
1085  }
1086  else
1087  p[i] = links[i].p;
1088  }
1089  } else if(q.Nrows() == 1 && q.Ncols() == dof) {
1090  for(int i = 1; i <= dof; i++)
1091  {
1092  links[i].transform(q(1,i));
1093  if(links[1].DH)
1094  {
1095  p[i](1) = links[i].get_a();
1096  p[i](2) = links[i].get_d() * links[i].R(3,2);
1097  p[i](3) = links[i].get_d() * links[i].R(3,3);
1098  }
1099  else
1100  p[i] = links[i].p;
1101  }
1102  } else if(q.Nrows() == adof && q.Ncols() == 1) {
1103  int j=1;
1104  for(int i = 1; i <= dof; i++)
1105  if(!links[i].immobile) {
1106  links[i].transform(q(j++,1));
1107  if(links[1].DH)
1108  {
1109  p[i](1) = links[i].get_a();
1110  p[i](2) = links[i].get_d() * links[i].R(3,2);
1111  p[i](3) = links[i].get_d() * links[i].R(3,3);
1112  }
1113  else
1114  p[i] = links[i].p;
1115  }
1116  } else if(q.Nrows() == 1 && q.Ncols() == adof) {
1117  int j=1;
1118  for(int i = 1; i <= dof; i++)
1119  if(!links[i].immobile) {
1120  links[i].transform(q(1,j++));
1121  if(links[1].DH)
1122  {
1123  p[i](1) = links[i].get_a();
1124  p[i](2) = links[i].get_d() * links[i].R(3,2);
1125  p[i](3) = links[i].get_d() * links[i].R(3,3);
1126  }
1127  else
1128  p[i] = links[i].p;
1129  }
1130  } else error("q has the wrong dimension in set_q()");
1131 }
1132 
1133 void Robot_basic::set_q(const ColumnVector & q)
1141 {
1142  if(q.Nrows() == dof) {
1143  for(int i = 1; i <= dof; i++)
1144  {
1145  links[i].transform(q(i));
1146  if(links[1].DH)
1147  {
1148  p[i](1) = links[i].get_a();
1149  p[i](2) = links[i].get_d() * links[i].R(3,2);
1150  p[i](3) = links[i].get_d() * links[i].R(3,3);
1151  }
1152  else
1153  p[i] = links[i].p;
1154  }
1155  } else if(q.Nrows() == get_available_dof()) {
1156  int j=1;
1157  for(int i = 1; i <= dof; i++)
1158  if(!links[i].immobile) {
1159  links[i].transform(q(j++));
1160  if(links[1].DH)
1161  {
1162  p[i](1) = links[i].get_a();
1163  p[i](2) = links[i].get_d() * links[i].R(3,2);
1164  p[i](3) = links[i].get_d() * links[i].R(3,3);
1165  }
1166  else
1167  p[i] = links[i].p;
1168  }
1169  } else error("q has the wrong dimension in set_q()");
1170 }
1171 
1172 void Robot_basic::set_qp(const ColumnVector & qp)
1174 {
1175  if(qp.Nrows() == dof)
1176  for(int i = 1; i <= dof; i++)
1177  links[i].qp = qp(i);
1178  else if(qp.Nrows() == get_available_dof()) {
1179  int j=1;
1180  for(int i = 1; i <= dof; i++)
1181  if(!links[i].immobile)
1182  links[i].qp = qp(j++);
1183  } else
1184  error("qp has the wrong dimension in set_qp()");
1185 }
1186 
1187 void Robot_basic::set_qpp(const ColumnVector & qpp)
1189 {
1190  if(qpp.Nrows() == dof)
1191  for(int i = 1; i <= dof; i++)
1192  links[i].qpp = qpp(i);
1193  else
1194  error("qpp has the wrong dimension in set_qpp()");
1195 }
1196 
1197 
1198 void Robot_basic::cleanUpPointers()
1199 
1202 {
1203  try
1204  {
1205  links = links+1;
1206  delete []links;
1207  delete []R;
1208  delete []dp;
1209  delete []dN;
1210  delete []dF;
1211  delete []dn;
1212  delete []df;
1213  delete []da;
1214  delete []dvp;
1215  delete []dwp;
1216  delete []dw;
1217  delete []pp;
1218  delete []p;
1219  delete []N;
1220  delete []F;
1221  delete []n_nv;
1222  delete []n;
1223  delete []f_nv;
1224  delete []f;
1225  delete []a;
1226  delete []vp;
1227  delete []wp;
1228  delete []w;
1229  }
1230  catch(...) {}
1231 }
1232 
1233 
1238 Robot::Robot(const int ndof): Robot_basic(ndof, true, false)
1239 {
1240  robotType_inv_kin();
1241 }
1242 
1247 Robot::Robot(const Matrix & dhinit): Robot_basic(dhinit, true, false)
1248 {
1249  robotType_inv_kin();
1250 }
1251 
1256 Robot::Robot(const Matrix & initrobot, const Matrix & initmotor):
1257  Robot_basic(initrobot, initmotor, true, false)
1258 {
1259  robotType_inv_kin();
1260 }
1261 
1266 Robot::Robot(const string & filename, const string & robotName):
1267  Robot_basic(filename, robotName, true, false)
1268 {
1269  robotType_inv_kin();
1270 }
1271 
1276 Robot::Robot(const Robot & x) : Robot_basic(x)
1277 {
1278 }
1279 
1280 void Robot::robotType_inv_kin()
1290 {
1291  if ( Puma_DH(*this) == true)
1292  robotType = PUMA;
1293  else if ( Rhino_DH(*this) == true)
1294  robotType = RHINO;
1295  else if (Schilling_DH(*this) == true )
1296  robotType = SCHILLING;
1297  else
1298  robotType = DEFAULT;
1299 }
1300 
1301 // ----------------- M O D I F I E D D H N O T A T I O N -------------------
1302 
1307 mRobot::mRobot(const int ndof) : Robot_basic(ndof, false, false)
1308 {
1309  robotType_inv_kin();
1310 }
1311 
1316 mRobot::mRobot(const Matrix & dhinit) : Robot_basic(dhinit, false, false)
1317 {
1318  robotType_inv_kin();
1319 }
1320 
1325 mRobot::mRobot(const Matrix & initrobot, const Matrix & initmotor) :
1326  Robot_basic(initrobot, initmotor, false, false)
1327 {
1328  robotType_inv_kin();
1329 }
1330 
1335 mRobot::mRobot(const string & filename, const string & robotName):
1336  Robot_basic(filename, robotName, false, false)
1337 {
1338  robotType_inv_kin();
1339 }
1340 
1345 mRobot::mRobot(const mRobot & x) : Robot_basic(x)
1346 {
1347  robotType_inv_kin();
1348 }
1349 
1350 void mRobot::robotType_inv_kin()
1360 {
1361  if ( Puma_mDH(*this) == true )
1362  robotType = PUMA;
1363  else if ( Rhino_mDH(*this) == true)
1364  robotType = RHINO;
1365  else if (Schilling_mDH(*this) == true)
1366  robotType = SCHILLING;
1367  else
1368  robotType = DEFAULT;
1369 }
1370 
1375 mRobot_min_para::mRobot_min_para(const int ndof) : Robot_basic(ndof, false, true)
1376 {
1377  robotType_inv_kin();
1378 }
1379 
1384 mRobot_min_para::mRobot_min_para(const Matrix & dhinit):
1385  Robot_basic(dhinit, false, true)
1386 {
1387  robotType_inv_kin();
1388 }
1389 
1394 mRobot_min_para::mRobot_min_para(const Matrix & initrobot, const Matrix & initmotor) :
1395  Robot_basic(initrobot, initmotor, false, true)
1396 {
1397  robotType_inv_kin();
1398 }
1399 
1404 mRobot_min_para::mRobot_min_para(const mRobot_min_para & x) : Robot_basic(x)
1405 {
1406  robotType_inv_kin();
1407 }
1408 
1413 mRobot_min_para::mRobot_min_para(const string & filename, const string & robotName):
1414  Robot_basic(filename, robotName, false, true)
1415 {
1416  robotType_inv_kin();
1417 }
1418 
1419 void mRobot_min_para::robotType_inv_kin()
1429 {
1430  if ( Puma_mDH(*this) == true)
1431  robotType = PUMA;
1432  else if ( Rhino_mDH(*this) == true)
1433  robotType = RHINO;
1434  else if (Schilling_mDH(*this) == true)
1435  robotType = SCHILLING;
1436  else
1437  robotType = DEFAULT;
1438 }
1439 
1440 // ---------------------- Non Member Functions -------------------------------
1441 
1442 void perturb_robot(Robot_basic & robot, const double f)
1451 {
1452  if( (f < 0) || (f > 1) )
1453  {
1454  cerr << "perturb_robot: f is not between 0 and 1" << endl;
1455  return;
1456  }
1457 
1458  double fact;
1459  srand(clock());
1460  for(int i = 1; i <= robot.get_dof()+robot.get_fix(); i++)
1461  {
1462  fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1463  robot.links[i].set_Im(robot.links[i].get_Im()*fact);
1464  fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1465  robot.links[i].set_B(robot.links[i].get_B()*fact);
1466  fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1467  robot.links[i].set_Cf(robot.links[i].get_Cf()*fact);
1468  fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1469  robot.links[i].set_m(robot.links[i].get_m()*fact);
1470  // fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1471  // robot.links[i].mc = robot.links[i].mc*fact;
1472  fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1473  Matrix I = robot.links[i].get_I()*fact;
1474  robot.links[i].set_I(I);
1475  }
1476 }
1477 
1478 
1479 bool Rhino_DH(const Robot_basic & robot)
1487 {
1488  if (robot.get_dof() == 5)
1489  {
1490  double a[6], d[6], alpha[6];
1491  for (int j = 1; j <= 5; j++)
1492  {
1493  if (robot.links[j].get_joint_type()) // All joints are rotoide
1494  {
1495  return false;
1496  }
1497  a[j] = robot.links[j].get_a();
1498  d[j] = robot.links[j].get_d();
1499  alpha[j] = robot.links[j].get_alpha();
1500  }
1501 
1502  if ( isZero(a[1]) && isZero(a[5]) && isZero(d[2]) && isZero(d[3]) &&
1503  isZero(d[4]) && isZero(alpha[2]) && isZero(alpha[3]) && isZero(alpha[5]))
1504  {
1505  return true;
1506  }
1507  }
1508  return false;
1509 }
1510 
1511 
1512 bool Puma_DH(const Robot_basic & robot)
1520 {
1521  if (robot.get_dof() == 6)
1522  {
1523  double a[7], d[7], alpha[7];
1524  for (int j = 1; j <= 6; j++)
1525  {
1526  if (robot.links[j].get_joint_type()) // All joints are rotoide
1527  {
1528  return false;
1529  }
1530  a[j] = robot.links[j].get_a();
1531  d[j] = robot.links[j].get_d();
1532  alpha[j] = robot.links[j].get_alpha();
1533  }
1534 
1535  // comparaison pour alpha de 90 a faire.
1536  if( isZero(a[1]) && a[2] && a[3] && isZero(a[4]) && isZero(a[5]) && isZero(a[6]) &&
1537  isZero(d[5]) && isZero(alpha[2]) && isZero(alpha[6]))
1538  {
1539  return true;
1540  }
1541  }
1542  return false;
1543 }
1544 
1545 bool Schilling_DH(const Robot_basic & robot)
1553 {
1554  if (robot.get_dof() == 6)
1555  {
1556  double a[7], d[7], alpha[7];
1557  for (int j = 1; j <= 6; j++)
1558  {
1559  if (robot.links[j].get_joint_type()) // All joints are rotoide
1560  return false;
1561  a[j] = robot.links[j].get_a();
1562  d[j] = robot.links[j].get_d();
1563  alpha[j] = robot.links[j].get_alpha();
1564  }
1565 
1566  // comparaison pour alpha de 90 a faire.
1567  if( isZero(a[5]) && isZero(a[6]) && isZero(d[2]) && isZero(d[3]) &&
1568  isZero(d[4]) && isZero(d[5]) && isZero(alpha[2]) && isZero(alpha[3]) &&
1569  isZero(alpha[6]))
1570  {
1571  return true;
1572  }
1573  }
1574 
1575  return false;
1576 }
1577 
1578 
1579 bool Rhino_mDH(const Robot_basic & robot)
1587 {
1588  if (robot.get_dof() == 5)
1589  {
1590  double a[6], d[6], alpha[6];
1591  for (int j = 1; j <= 5; j++)
1592  {
1593  if (robot.links[j].get_joint_type()) // All joints are rotoide
1594  {
1595  return false;
1596  }
1597  a[j] = robot.links[j].get_a();
1598  d[j] = robot.links[j].get_d();
1599  alpha[j] = robot.links[j].get_alpha();
1600  }
1601  // comparaison pour alpha de 90 a ajouter
1602  if ( isZero(a[1]) && isZero(a[2]) && isZero(d[2]) && isZero(d[3]) &&
1603  isZero(d[4]) && isZero(alpha[1]) && isZero(alpha[3]) && isZero(alpha[4]))
1604  {
1605  return true;
1606  }
1607  }
1608  return false;
1609 }
1610 
1611 bool Puma_mDH(const Robot_basic & robot)
1619 {
1620  if (robot.get_dof() == 6)
1621  {
1622  double a[7], d[7], alpha[7];
1623  for (int j = 1; j <= 6; j++)
1624  {
1625  if (robot.links[j].get_joint_type()) // All joints are rotoide
1626  {
1627  return false;
1628  }
1629  a[j] = robot.links[j].get_a();
1630  d[j] = robot.links[j].get_d();
1631  alpha[j] = robot.links[j].get_alpha();
1632  }
1633 
1634  // comparaison pour alpha de 90.
1635 
1636  if ( isZero(a[1]) && isZero(a[2]) && isZero(a[5]) && isZero(a[6]) &&
1637  isZero(d[5]) && isZero(alpha[1]) && isZero(alpha[3]))
1638  {
1639  return true;
1640  }
1641  }
1642  return false;
1643 }
1644 
1645 bool Schilling_mDH(const Robot_basic & robot)
1653 {
1654  if (robot.get_dof() == 6)
1655  {
1656  double a[7], d[7], alpha[7];
1657  for (int j = 1; j <= 6; j++)
1658  {
1659  if (robot.links[j].get_joint_type()) // All joints are rotoide
1660  {
1661  return false;
1662  }
1663  a[j] = robot.links[j].get_a();
1664  d[j] = robot.links[j].get_d();
1665  alpha[j] = robot.links[j].get_alpha();
1666  }
1667 
1668  // comparaison pour alpha de 90.
1669  if ( isZero(a[1]) && isZero(a[6]) && isZero(d[2]) && isZero(d[3]) && isZero(d[4]) &&
1670  isZero(d[5]) && isZero(alpha[1]) && isZero(alpha[3]) && isZero(alpha[4]))
1671  {
1672  return true;
1673  }
1674  }
1675  return false;
1676 }
1677 
1678 #ifdef use_namespace
1679 }
1680 #endif
1681 
1682