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


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