kinematics.cpp
Go to the documentation of this file.
1  /**************************************************************************
2 * kinematics.cpp -
3 * Kinematics class for Katana4XX Robots using kinematics lib
4 * Copyright (C) 2007-2008 Neuronics AG
5 * PKE/UKE 2007, JHA 2008
6  **************************************************************************/
7 
8 #include "kinematics.h"
9 
13 }
17  setType(type);
18  init();
19 }
22  delete _anaGuess;
23 }
25 
26 
27 int KinematicsLib::sign(int value) {
28  return (value > 0) - (value < 0);
29 }
30 int KinematicsLib::sign(double value) {
31  return (value > 0) - (value < 0);
32 }
33 #ifdef WIN32
34 double KinematicsLib::round( double x)
35 // Copyright (C) 2001 Tor M. Aamodt, University of Toronto
36 // Permisssion to use for all purposes commercial and otherwise granted.
37 // THIS MATERIAL IS PROVIDED "AS IS" WITHOUT WARRANTY, OR ANY CONDITION OR
38 // OTHER TERM OF ANY KIND INCLUDING, WITHOUT LIMITATION, ANY WARRANTY
39 // OF MERCHANTABILITY, SATISFACTORY QUALITY, OR FITNESS FOR A PARTICULAR
40 // PURPOSE.
41 {
42  if( x > 0 ) {
43  __int64 xint = (__int64) (x+0.5);
44  if( xint % 2 ) {
45  // then we might have an even number...
46  double diff = x - (double)xint;
47  if( diff == -0.5 )
48  return double(xint-1);
49  }
50  return double(xint);
51  } else {
52  __int64 xint = (__int64) (x-0.5);
53  if( xint % 2 ) {
54  // then we might have an even number...
55  double diff = x - (double)xint;
56  if( diff == 0.5 )
57  return double(xint+1);
58  }
59  return double(xint);
60  }
61 }
62 #endif // ifdef WIN32
64  _type = -1;
65  _matrixInit = false;
66  _dof = -1;
67  _dom = -1;
68  _angOffInit = false;
69  _angRanInit = false;
70  _immobile = 0;
71  _thetaimmobile = 0.0;
72  _initialized = false;
73  for (int i = 0; i < 4; ++i) {
74  _tcpOffset[i] = 0.0;
75  }
76  return 1;
77 }
79  int dir;
80  for (int i = 0; i < _dof; i++) {
81  dir = sign(_encoderOffset[i]) * _rotDir[i];
82  if (dir < 0) {
83  _angleMin[i] = _angleOffset[i];
84  _angleMax[i] = _angleOffset[i] + _angleRange[i];
85  } else {
86  _angleMax[i] = _angleOffset[i];
87  _angleMin[i] = _angleOffset[i] - _angleRange[i];
88  }
89  _data(i + 1, 6) = _angleMin[i];
90  _data(i + 1, 7) = _angleMax[i];
91  }
92 
93  return 1;
94 }
96  _dof = dof;
97  _dom = _dof;
98  _data = Matrix(_dof, 23);
99  _data = 0.0;
100  _matrixInit = true;
101 
102  return 1;
103 }
104 int KinematicsLib::angleArrMDH2vecK4D(const double arr[], std::vector<double>* angleK4D) {
105  if (_type < 0)
106  return -1;
107  std::vector<double> angleMDH;
108  for (int i = 0; i < _dom; ++i) {
109  angleMDH.push_back(arr[i]);
110  }
111  angleK4D->clear();
112  mDH2K4DAng(angleMDH, *angleK4D);
113  return 1;
114 }
115 
116 
118 int KinematicsLib::setType(int type) {
119  std::vector<double> angOff;
120  double angStopArr[MaxDof];
121  std::vector<double> angStop;
122  std::vector<double> lengths;
123  switch(type) {
124  case K_6M90A_F: // 0
125  _type = type;
128  for (int i = 0; i < _dof; i++) {
129  _data(i + 1, 3) *= LENGTH_MULTIPLIER;
130  _data(i + 1, 4) *= LENGTH_MULTIPLIER;
131  }
132  for (int i = 0; i < _dof; i++) {
133  _epc[i] = Encoder_per_cycle[i];
135  _rotDir[i] = Rotation_direction[i];
138  }
139  _angOffInit = true;
140  _angRanInit = true;
141  setAngleMinMax();
142  for (int i = 0; i < 4; i++) {
144  }
145  // analytical guess
148  _anaGuess->setAngOff(angOff);
149  for (int i = 0; i < _dom; ++i) {
150  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
151  _angleRange[i];
152  }
153  angleArrMDH2vecK4D(angStopArr, &angStop);
154  _anaGuess->setAngStop(angStop);
155  for (int i = 0; i < 4; ++i) {
156  lengths.push_back(_linkLength[i] * 1000);
157  }
158  _anaGuess->setLinkLength(lengths);
159  break;
160  case K_6M90A_G: // 1
161  _type = type;
164  for (int i = 0; i < _dof; i++) {
165  _data(i + 1, 3) *= LENGTH_MULTIPLIER;
166  _data(i + 1, 4) *= LENGTH_MULTIPLIER;
167  }
168  _dom = Dof_90 - 1;
169  _immobile = true;
170  _thetaimmobile = _data(_dof, 2);
171  for (int i = 0; i < _dof; i++) {
172  _epc[i] = Encoder_per_cycle[i];
174  _rotDir[i] = Rotation_direction[i];
177  }
178  _angOffInit = true;
179  _angRanInit = true;
180  setAngleMinMax();
181  for (int i = 0; i < 4; i++) {
183  }
184  // analytical guess
187  angOff.push_back(-2.150246); // immobile angle in mDH
188  _anaGuess->setAngOff(angOff);
189  for (int i = 0; i < _dom; ++i) {
190  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
191  _angleRange[i];
192  }
193  angleArrMDH2vecK4D(angStopArr, &angStop);
194  angStop.push_back(3.731514); // immobile angle in mDH
195  _anaGuess->setAngStop(angStop);
196  for (int i = 0; i < 4; ++i) {
197  lengths.push_back(_linkLength[i] * 1000);
198  }
199  _anaGuess->setLinkLength(lengths);
200  break;
201  case K_6M180: // 2
202  _type = type;
205  for (int i = 0; i < _dof; i++) {
206  _data(i + 1, 3) *= LENGTH_MULTIPLIER;
207  _data(i + 1, 4) *= LENGTH_MULTIPLIER;
208  }
209  for (int i = 0; i < _dof; i++) {
210  _epc[i] = Encoder_per_cycle[i];
212  _rotDir[i] = Rotation_direction[i];
215  }
216  _angOffInit = true;
217  _angRanInit = true;
218  setAngleMinMax();
219  for (int i = 0; i < 4; i++) {
221  }
222  // analytical guess
225  angOff.push_back(-2.150246); // angle not present in mDH
226  _anaGuess->setAngOff(angOff);
227  for (int i = 0; i < _dom; ++i) {
228  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
229  _angleRange[i];
230  }
231  angleArrMDH2vecK4D(angStopArr, &angStop);
232  angStop.push_back(3.731514); // angle not present in mDH
233  _anaGuess->setAngStop(angStop);
234  for (int i = 0; i < 4; ++i) {
235  lengths.push_back(_linkLength[i] * 1000);
236  }
237  _anaGuess->setLinkLength(lengths);
238  break;
239  case K_6M90B_F: // 3
240  _type = type;
243  for (int i = 0; i < _dof; i++) {
244  _data(i + 1, 3) *= LENGTH_MULTIPLIER;
245  _data(i + 1, 4) *= LENGTH_MULTIPLIER;
246  }
247  for (int i = 0; i < _dof; i++) {
248  _epc[i] = Encoder_per_cycle[i];
250  _rotDir[i] = Rotation_direction[i];
253  }
254  _angOffInit = true;
255  _angRanInit = true;
256  setAngleMinMax();
257  for (int i = 0; i < 4; i++) {
259  }
260  // analytical guess
263  _anaGuess->setAngOff(angOff);
264  for (int i = 0; i < _dom; ++i) {
265  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
266  _angleRange[i];
267  }
268  angleArrMDH2vecK4D(angStopArr, &angStop);
269  _anaGuess->setAngStop(angStop);
270  for (int i = 0; i < 4; ++i) {
271  lengths.push_back(_linkLength[i] * 1000);
272  }
273  _anaGuess->setLinkLength(lengths);
274  break;
275  case K_6M90B_G: // 4
276  _type = type;
279  for (int i = 0; i < _dof; i++) {
280  _data(i + 1, 3) *= LENGTH_MULTIPLIER;
281  _data(i + 1, 4) *= LENGTH_MULTIPLIER;
282  }
283  _dom = Dof_90 - 1;
284  _immobile = true;
285  _thetaimmobile = _data(_dof, 2);
286  for (int i = 0; i < _dof; i++) {
287  _epc[i] = Encoder_per_cycle[i];
289  _rotDir[i] = Rotation_direction[i];
292  }
293  _angOffInit = true;
294  _angRanInit = true;
295  setAngleMinMax();
296  for (int i = 0; i < 4; i++) {
298  }
299  // analytical guess
302  angOff.push_back(-2.150246); // immobile angle in mDH
303  _anaGuess->setAngOff(angOff);
304  for (int i = 0; i < _dom; ++i) {
305  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
306  _angleRange[i];
307  }
308  angleArrMDH2vecK4D(angStopArr, &angStop);
309  angStop.push_back(3.731514); // immobile angle in mDH
310  _anaGuess->setAngStop(angStop);
311  for (int i = 0; i < 4; ++i) {
312  lengths.push_back(_linkLength[i] * 1000);
313  }
314  _anaGuess->setLinkLength(lengths);
315  break;
316  default:
317  return -1;
318  }
319 
320  return 1;
321 }
323 int KinematicsLib::setMDH(std::vector<double> theta, std::vector<double> d,
324  std::vector<double> a, std::vector<double> alpha, int typeNr) {
325  // check vector sizes
326  if (_dof == -1) {
327  if ((int)theta.size() > MaxDof)
328  return -1;
329  initDofMat(theta.size());
330  }
331 
332  if ((int)theta.size() != _dof || (int)d.size() != _dof ||
333  (int)a.size() != _dof || (int)alpha.size() != _dof) {
334  return -1;
335  }
336 
337  if (typeNr >= 0)
338  typeNr = -2;
339 
340  for (int i = 0; i < _dof; ++i) {
341  _data(i + 1, 2) = theta.at(i);
342  _data(i + 1, 3) = d.at(i) * LENGTH_MULTIPLIER;
343  _data(i + 1, 4) = a.at(i) * LENGTH_MULTIPLIER;
344  _data(i + 1, 5) = alpha.at(i);
345  _data(i + 1, 23) = 0;
346  }
347 
348  _dom = _dof;
349  _immobile = false;
350  _type = typeNr;
351 
352  return 1;
353 }
355 int KinematicsLib::setLinkLen(std::vector<double> links) {
356  if ((_dof == -1) || ((int)links.size() != 4))
357  return -1;
358 
359  switch (_type) {
360  case K_6M90A_F: // 0
361  case K_6M90A_G: // 1
362  case K_6M90B_F: // 3
363  case K_6M90B_G: // 4
364  _data(3, 4) = links.at(0) * LENGTH_MULTIPLIER;
365  _data(4, 4) = links.at(1) * LENGTH_MULTIPLIER;
366  _data(5, 3) = links.at(2) * LENGTH_MULTIPLIER;
367  _data(6, 3) = links.at(3) * LENGTH_MULTIPLIER;
368  break;
369  case K_6M180: // 2
370  _data(3, 4) = links.at(0) * LENGTH_MULTIPLIER;
371  _data(4, 4) = links.at(1) * LENGTH_MULTIPLIER;
372  _data(5, 3) = (links.at(2) + links.at(3)) * LENGTH_MULTIPLIER;
373  break;
374  default:
375  return -1;
376  }
377 
378  // store in _linkLength
379  for (int i = 0; i < 4; ++i) {
380  _linkLength[i] = links.at(i);
381  }
382 
383  // set in AnalyticalGuess
384  std::vector<double> lengths;
385  for (int i = 0; i < 4; ++i) {
386  lengths.push_back(_linkLength[i] * 1000);
387  }
388  _anaGuess->setLinkLength(lengths);
389 
390  return 1;
391 }
393 int KinematicsLib::setImmob(int immob) {
394  if (_dof == -1 || immob < 0 || immob > 1) {
395  return -1;
396  }
397 
398  _data(_dof, 23) = immob;
399  _immobile = immob;
400  if (immob) {
401  _dom = _dof - 1;
402  _thetaimmobile = _data(_dof, 2);
403  } else {
404  _dom = _dof;
405  }
406 
407  return 1;
408 }
410 int KinematicsLib::setEPC(std::vector<int> epc) {
411  if ((int)epc.size() < _dom) {
412  return -1;
413  }
414 
415  for (int i = 0; i < _dom; ++i) {
416  _epc[i] = epc.at(i);
417  }
418 
419  return 1;
420 }
422 int KinematicsLib::setEncOff(std::vector<int> encOffset) {
423  if ((int)encOffset.size() < _dom) {
424  return -1;
425  }
426 
427  for (int i = 0; i < _dom; ++i) {
428  _encoderOffset[i] = encOffset.at(i);
429  }
430 
431  return 1;
432 }
434 int KinematicsLib::setRotDir(std::vector<int> rotDir) {
435  if ((int)rotDir.size() < _dom) {
436  return -1;
437  }
438 
439  for (int i = 0; i < _dom; ++i) {
440  if (rotDir.at(i) < 0) {
441  _rotDir[i] = -1;
442  } else {
443  _rotDir[i] = 1;
444  }
445  }
446 
447  return 1;
448 }
450 int KinematicsLib::setAngOff(std::vector<double> angleOffset) {
451  if ((int)angleOffset.size() < _dom) {
452  return -1;
453  }
454 
455  for (int i = 0; i < _dom; ++i) {
456  _angleOffset[i] = angleOffset.at(i);
457  }
458  _angOffInit = true;
459  if (_angRanInit)
460  setAngleMinMax();
461 
462  // analytical guess
463  std::vector<double> angOff;
464  double angStopArr[MaxDof];
465  std::vector<double> angStop;
466  switch(_type) {
467  case K_6M90A_F: // 0
468  case K_6M90B_F: // 3
470  _anaGuess->setAngOff(angOff);
471  for (int i = 0; i < _dom; ++i) {
472  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
473  _angleRange[i];
474  }
475  angleArrMDH2vecK4D(angStopArr, &angStop);
476  _anaGuess->setAngStop(angStop);
477  break;
478  case K_6M90A_G: // 1
479  case K_6M90B_G: // 4
481  angOff.push_back(-2.150246); // immobile angle in mDH
482  _anaGuess->setAngOff(angOff);
483  for (int i = 0; i < _dom; ++i) {
484  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
485  _angleRange[i];
486  }
487  angleArrMDH2vecK4D(angStopArr, &angStop);
488  angStop.push_back(3.731514); // immobile angle in mDH
489  _anaGuess->setAngStop(angStop);
490  break;
491  case K_6M180: // 2
493  angOff.push_back(-2.150246); // angle not present in mDH
494  _anaGuess->setAngOff(angOff);
495  for (int i = 0; i < _dom; ++i) {
496  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
497  _angleRange[i];
498  }
499  angleArrMDH2vecK4D(angStopArr, &angStop);
500  angStop.push_back(3.731514); // angle not present in mDH
501  _anaGuess->setAngStop(angStop);
502  break;
503  default:
504  break;
505  }
506 
507  return 1;
508 }
510 int KinematicsLib::setAngRan(std::vector<double> angleRange) {
511  if ((int)angleRange.size() < _dom) {
512  return -1;
513  }
514 
515  for (int i = 0; i < _dom; ++i) {
516  _angleRange[i] = angleRange.at(i);
517  }
518  _angRanInit = true;
519  if (_angOffInit)
520  setAngleMinMax();
521 
522  // analytical guess
523  double angStopArr[MaxDof];
524  std::vector<double> angStop;
525  switch(_type) {
526  case K_6M90A_F: // 0
527  case K_6M90B_F: // 3
528  for (int i = 0; i < _dom; ++i) {
529  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
530  _angleRange[i];
531  }
532  angleArrMDH2vecK4D(angStopArr, &angStop);
533  _anaGuess->setAngStop(angStop);
534  break;
535  case K_6M90A_G: // 1
536  case K_6M90B_G: // 4
537  for (int i = 0; i < _dom; ++i) {
538  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
539  _angleRange[i];
540  }
541  angleArrMDH2vecK4D(angStopArr, &angStop);
542  angStop.push_back(3.731514); // immobile angle in mDH
543  _anaGuess->setAngStop(angStop);
544  break;
545  case K_6M180: // 2
546  for (int i = 0; i < _dom; ++i) {
547  angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
548  _angleRange[i];
549  }
550  angleArrMDH2vecK4D(angStopArr, &angStop);
551  angStop.push_back(3.731514); // angle not present in mDH
552  _anaGuess->setAngStop(angStop);
553  break;
554  default:
555  break;
556  }
557 
558  return 1;
559 }
561 int KinematicsLib::setTcpOff(std::vector<double> tcpOffset) {
562  if ((int)tcpOffset.size() < 4) {
563  return -1;
564  }
565 
566  for (int i = 0; i < 4; ++i) {
567  _tcpOffset[i] = tcpOffset.at(i);
568  }
569 
570  return 1;
571 }
574  return _type;
575 }
578  return MaxDof;
579 }
582  return _dof;
583 }
586  return _dom;
587 }
589 int KinematicsLib::getMDH(std::vector<double>& theta, std::vector<double>& d,
590  std::vector<double>& a, std::vector<double>& alpha) {
591  if (_dof == -1)
592  return -1;
593  theta.clear();
594  d.clear();
595  a.clear();
596  alpha.clear();
597  for (int i = 0; i < _dof; ++i) {
598  theta.push_back(_data(i + 1, 2));
599  d.push_back(_data(i + 1, 3) / LENGTH_MULTIPLIER);
600  a.push_back(_data(i + 1, 4) / LENGTH_MULTIPLIER);
601  alpha.push_back(_data(i + 1, 5));
602  }
603 
604  return 1;
605 }
608  return _immobile;
609 }
611 int KinematicsLib::getEPC(std::vector<int>& epc) {
612  if (_dof == -1)
613  return -1;
614  epc.clear();
615  for (int i = 0; i < _dom; ++i) {
616  epc.push_back(_epc[i]);
617  }
618 
619  return 1;
620 }
622 int KinematicsLib::getEncOff(std::vector<int>& encOffset) {
623  if (_dof == -1)
624  return -1;
625  encOffset.clear();
626  for (int i = 0; i < _dom; ++i) {
627  encOffset.push_back(_encoderOffset[i]);
628  }
629 
630  return 1;
631 }
633 int KinematicsLib::getRotDir(std::vector<int>& rotDir) {
634  if (_dof == -1)
635  return -1;
636  rotDir.clear();
637  for (int i = 0; i < _dom; ++i) {
638  rotDir.push_back(_rotDir[i]);
639  }
640 
641  return 1;
642 }
644 int KinematicsLib::getAngOff(std::vector<double>& angleOffset) {
645  if (_dof == -1)
646  return -1;
647  angleOffset.clear();
648  for (int i = 0; i < _dom; ++i) {
649  angleOffset.push_back(_angleOffset[i]);
650  }
651 
652  return 1;
653 }
655 int KinematicsLib::getAngRan(std::vector<double>& angleRange) {
656  if (_dof == -1)
657  return -1;
658  angleRange.clear();
659  for (int i = 0; i < _dom; ++i) {
660  angleRange.push_back(_angleRange[i]);
661  }
662 
663  return 1;
664 }
666 int KinematicsLib::getAngStop(std::vector<double>& angleStop) {
667  std::vector<double> angoff;
668  int okcount = getAngOff(angoff);
669  std::vector<int> encoff;
670  okcount += getEncOff(encoff);
671  std::vector<int> rotdir;
672  okcount += getRotDir(rotdir);
673  std::vector<double> angran;
674  okcount += getAngRan(angran);
675  angleStop.clear();
676  for (int i = 0; i < _dom; i++) {
677  angleStop.push_back(angoff.at(i) - (sign(encoff.at(i)) * rotdir.at(i))
678  * (angran.at(i)));
679  }
680  int ok = (okcount == 4) ? 1 : 0;
681  return ok;
682 }
684 int KinematicsLib::getAngMin(std::vector<double>& angleMin) {
685  std::vector<double> angoff;
686  int okcount = getAngOff(angoff);
687  std::vector<double> angstop;
688  okcount += getAngStop(angstop);
689  angleMin.clear();
690  for (int i = 0; i < _dom; ++i) {
691  angleMin.push_back(min(angoff.at(i), angstop.at(i)));
692  }
693  int ok = (okcount == 2) ? 1 : 0;
694  return ok;
695 }
697 int KinematicsLib::getAngMax(std::vector<double>& angleMax) {
698  std::vector<double> angoff;
699  int okcount = getAngOff(angoff);
700  std::vector<double> angstop;
701  okcount += getAngStop(angstop);
702  angleMax.clear();
703  for (int i = 0; i < _dom; ++i) {
704  angleMax.push_back(max(angoff.at(i), angstop.at(i)));
705  }
706  int ok = (okcount == 2) ? 1 : 0;
707  return ok;
708 }
710 int KinematicsLib::getTcpOff(std::vector<double>& tcpOffset) {
711  if (_dof == -1)
712  return -1;
713  tcpOffset.clear();
714  for (int i = 0; i < 4; ++i) {
715  tcpOffset.push_back(_tcpOffset[i]);
716  }
717 
718  return 1;
719 }
721 int KinematicsLib::getVersion(std::vector<int>& version) {
722  version.clear();
723  version.push_back(KINLIB_VERSION_MAJOR);
724  version.push_back(KINLIB_VERSION_MINOR);
725  version.push_back(KINLIB_VERSION_REVISION);
726 
727  return 1;
728 }
731  if (!_matrixInit || !_angOffInit || !_angRanInit)
732  return -1;
733 
734  _robot = mRobot(_data);
735  _initialized = true;
736 
737  return 1;
738 }
740 int KinematicsLib::K4D2mDHAng(std::vector<double> angleK4D, std::vector<double>& angleMDH) {
741  if (_type == -1)
742  return -1;
743 
744  if ((int)angleK4D.size() < _dom)
745  return -1;
746 
747  angleMDH.clear();
748 
749  // for all models the same
750  angleMDH.push_back(angleK4D.at(0) - mPi);
751  angleMDH.push_back(angleK4D.at(1));
752  angleMDH.push_back(angleK4D.at(2) - mPi);
753  angleMDH.push_back(mPi / 2.0 - angleK4D.at(3));
754 
755  // model specific
756  switch (_type) {
757  case K_6M90A_F: // 0
758  case K_6M90B_F: // 3
759  angleMDH.push_back(mPi / 2.0 - angleK4D.at(4));
760  angleMDH.push_back(mPi / 2.0 - angleK4D.at(5));
761  break;
762  case K_6M90A_G: // 1
763  case K_6M90B_G: // 4
764  angleMDH.push_back(mPi / 2.0 - angleK4D.at(4));
765  break;
766  case K_6M180: // 2
767  angleMDH.push_back(-1.0 * angleK4D.at(4) + 3.0 * mPi / 2.0);
768  break;
769  default:
770  return -1;
771  }
772 
773  return 1;
774 }
776 int KinematicsLib::mDH2K4DAng(std::vector<double> angleMDH, std::vector<double>& angleK4D) {
777  if (_type == -1)
778  return -1;
779 
780  if ((int)angleMDH.size() < _dom)
781  return -1;
782 
783  angleK4D.clear();
784 
785  // for all models the same
786  angleK4D.push_back(angleMDH.at(0) + mPi);
787  angleK4D.push_back(angleMDH.at(1));
788  angleK4D.push_back(angleMDH.at(2) + mPi);
789  angleK4D.push_back(mPi / 2.0 - angleMDH.at(3));
790 
791  // model specific
792  switch (_type) {
793  case K_6M90A_F: // 0
794  case K_6M90B_F: // 3
795  angleK4D.push_back(mPi / 2.0 - angleMDH.at(4));
796  angleK4D.push_back(mPi / 2.0 - angleMDH.at(5));
797  break;
798  case K_6M90A_G: // 1
799  case K_6M90B_G: // 4
800  angleK4D.push_back(mPi / 2.0 - angleMDH.at(4));
801  break;
802  case K_6M180: // 2
803  angleK4D.push_back(-1.0 * angleMDH.at(4) + 3.0 * mPi / 2.0);
804  break;
805  default:
806  return -1;
807  }
808 
809  return 1;
810 }
812 int KinematicsLib::enc2rad(std::vector<int> encoders,
813  std::vector<double>& angles) {
814  if ((int)encoders.size() < _dom)
815  return -1;
816 
817  angles.clear();
818  for(int i = 0; i < _dom; ++i) {
819  angles.push_back(_angleOffset[i] + _rotDir[i] * (encoders.at(i) -
820  _encoderOffset[i]) * 2.0 * mPi / ((double) _epc[i]));
821  }
822 
823  return 1;
824 }
826 int KinematicsLib::rad2enc(std::vector<double> angles,
827  std::vector<int>& encoders) {
828  if ((int)angles.size() < _dom)
829  return -1;
830 
831  encoders.clear();
832  for(int i = 0; i < _dom; ++i){
833  encoders.push_back((int) round(_encoderOffset[i] + _rotDir[i] * (angles.at(i) -
834  _angleOffset[i]) * _epc[i] / (2 * mPi)));
835  }
836 
837  return 1;
838 }
840 int KinematicsLib::directKinematics(std::vector<double> angles,
841  std::vector<double>& pose) {
842  if (!_initialized || (int)angles.size() < _dom)
843  return -1;
844 
845  //Angles in ColumnVector
847  for(int k = 0; k < _dof; ++k){
848  if (k == _dom) {
849  // immobile = 1: _dom == _dof - 1
850  qr(k + 1) = (float) _thetaimmobile;
851  } else {
852  qr(k + 1) = angles.at(k);
853  }
854  }
855  _robot.set_q(qr);
856 
857  //Calculate kinematics
858  Matrix TCP_Position = _robot.kine();
859  TCP_Position(1,4) /= LENGTH_MULTIPLIER;
860  TCP_Position(2,4) /= LENGTH_MULTIPLIER;
861  TCP_Position(3,4) /= LENGTH_MULTIPLIER;
862 
863  // adjust TCP offset
864  // transformation from flange (kinematics tcp) to effective tcp (with offset)
865  Matrix TCP_Offset(4, 4);
866  TCP_Offset.row(1) << 1.0 << 0.0 << 0.0 << _tcpOffset[0];
867  TCP_Offset.row(2) << 0.0 << cos(_tcpOffset[3]) << -sin(_tcpOffset[3]) << _tcpOffset[1];
868  TCP_Offset.row(3) << 0.0 << sin(_tcpOffset[3]) << cos(_tcpOffset[3]) << _tcpOffset[2];
869  TCP_Offset.row(4) << 0.0 << 0.0 << 0.0 << 1.0;
870  TCP_Position = TCP_Position * TCP_Offset;
871 
872  pose.clear();
873 
874  //x-Position
875  pose.push_back(TCP_Position(1,4));
876  //y-Position
877  pose.push_back(TCP_Position(2,4));
878  //z-Position
879  pose.push_back(TCP_Position(3,4));
880 
881  //Calculate euler angles
882  ColumnVector eul = ieulzxz(TCP_Position);
883 
884  //Phi
885  pose.push_back(eul(1));
886  //Theta
887  pose.push_back(eul(2));
888  //Psi
889  pose.push_back(eul(3));
890 
891  return 1;
892 }
893 
895 
896 // single inverse kinematics
897 int KinematicsLib::invKin(std::vector<double> pose, std::vector<double> prev,
898  std::vector<double>& angle) {
899  if ((int)pose.size() < 6 || (int)prev.size() < _dof)
900  return -1;
901 
902  // IK algorithm type to use
903  const int ikalgo = 0; // based on Jacobian (faster)
904  //const int ikalgo = 1; // based on derivative of T (converge more often)
905 
906  //ReturnMatrix eulzxz(const ColumnVector & a);
907  ColumnVector v(3);
908  v(1) = pose.at(3);
909  v(2) = pose.at(4);
910  v(3) = pose.at(5);
911  Matrix Pos = eulzxz(v);
912  Pos(1,4) = pose.at(0) * LENGTH_MULTIPLIER;
913  Pos(2,4) = pose.at(1) * LENGTH_MULTIPLIER;
914  Pos(3,4) = pose.at(2) * LENGTH_MULTIPLIER;
915 
916  //Set previous angles
918  for (int j = 0; j < _dof; ++j) {
919  qs(j+1) = prev.at(j);
920  }
921  _robot.set_q(qs);
922 
923  bool converge = false;
924  ColumnVector q0 = _robot.inv_kin(Pos, ikalgo, _dof, converge);
925 
926  angle.clear();
927  for (int j = 0; j < _dom; ++j) {
928  angle.push_back(q0(j + 1));
929  }
930  if (_immobile == 1)
931  angle.push_back(_thetaimmobile);
932 
933  int ok = 1;
934  if (!converge)
935  ok = -1;
936  return ok;
937 }
938 
939 // inverse kinematics using bisection if no solution found
940 int KinematicsLib::invKin_bisec(std::vector<double> pose, std::vector<double> prev,
941  std::vector<double>& conf, int maxBisection) {
942  if ((int)pose.size() < 6 || (int)prev.size() < _dof || maxBisection < 0)
943  return -1;
944 
945  int ok = invKin(pose, prev, conf);
946 
947  // Orig 3D pose
948  // Orig JS prev angle
949  // Bisec 3D prev1pose prev2pose pose
950  // Bisec JS prev prev2conf conf
951  if (ok < 0 && maxBisection > 0) {
952  // prev1pose
953  std::vector<double> prev1pose;
954  directKinematics(prev, prev1pose);
955 
956  // prev2pose
957  std::vector<double> prev2pose;
958  for (int i = 0; i < 6; ++i)
959  prev2pose.push_back(prev1pose.at(i) + pose.at(i) / 2.0);
960 
961  // prev2conf (IK on first part)
962  std::vector<double> prev2conf;
963  ok = inverseKinematics(prev2pose, prev, prev2conf, maxBisection-1);
964 
965  if (ok == 1) {
966  // conf (IK on second part, only if first part successful)
967  ok = inverseKinematics(pose, prev2conf, conf, maxBisection-1);
968  }
969  }
970 
971  return ok;
972 }
973 
974 // analytical guess
975 int KinematicsLib::anaGuess(std::vector<double> pose, std::vector<double> prev,
976  std::vector<double>& angle) {
977  if (_type < 0 || (int)pose.size() < 6 || (int)prev.size() < _dof)
978  return -1;
979 
980  std::vector<double> positions;
981  for (int i = 0; i < 6; ++i) {
982  if (i < 3)
983  positions.push_back(pose.at(i) * 1000);
984  else
985  positions.push_back(pose.at(i));
986  }
987 
988  std::vector<double> previous_angles;
989  std::vector<double> prevK4D;
990  mDH2K4DAng(prev, prevK4D);
991  for (int i = 0; i < _dom; ++i) {
992  previous_angles.push_back(prevK4D.at(i));
993  }
994  if (_dom == 5) {
995  // give 6 previous angles to the analytical kinematics
996  previous_angles.push_back(0.0);
997  }
998 
999  std::vector<double> anglesK4D;
1000 
1001  int error;
1002  try{
1003  error = ((int) _anaGuess->inverseKinematics(anglesK4D, positions,
1004  previous_angles)) - 1;
1005  K4D2mDHAng(anglesK4D, angle);
1006  if (_immobile)
1007  angle.push_back(_thetaimmobile);
1008  } catch (AnaGuess::NoSolutionException nse) {
1009  error = -2;
1010  } catch (AnaGuess::Exception e) {
1011  error = e.errorNumber() - 2;
1012  }
1013 
1014  if (error != 0) {
1015  angle.clear();
1016  for (int i = 0; i < _dof; ++i)
1017  angle.push_back(prev.at(i));
1018  }
1019 
1020  int ok = (error == 0) ? 1 : -1;
1021  return ok;
1022 }
1023 
1024 bool KinematicsLib::checkConfig(std::vector<double> config, std::vector<double> pose,
1025  double tol) {
1026  std::vector<double> configpose;
1027  directKinematics(config, configpose);
1028  double posdiff = 0.0;
1029  for (int i = 0; i < 6; ++i) {
1030  posdiff += abs(pose.at(i) - configpose.at(i));
1031  }
1032  bool ok;
1033  if (posdiff > tol)
1034  ok = false;
1035  else
1036  ok = true;
1037  return ok;
1038 }
1039 
1041 
1043 int KinematicsLib::inverseKinematics(std::vector<double> pose,
1044  std::vector<double> prev, std::vector<double>& angles, int maxBisection) {
1045  if (!_initialized || (int)pose.size() < 6 || (int)prev.size() < _dom ||
1046  maxBisection < 0)
1047  return -1;
1048 
1049  // tolerance for configuration check (sum of 3 * meter + 3 * radian)
1050  double tol = 0.0001;
1051 
1052  // adjust TCP offset
1053  // pose to matrix
1054  ColumnVector v(3);
1055  v(1) = pose.at(3);
1056  v(2) = pose.at(4);
1057  v(3) = pose.at(5);
1058  Matrix Pos = eulzxz(v);
1059  Pos(1,4) = pose.at(0);
1060  Pos(2,4) = pose.at(1);
1061  Pos(3,4) = pose.at(2);
1062  // transformation from effective tcp (with offset) to flange (kinematics tcp)
1063  Matrix TCP_Offset(4, 4);
1064  TCP_Offset.row(1) << 1.0 << 0.0 << 0.0 << -_tcpOffset[0];
1065  TCP_Offset.row(2) << 0.0 << cos(_tcpOffset[3]) << sin(_tcpOffset[3]) << (-_tcpOffset[1] * cos(_tcpOffset[3]) - _tcpOffset[2] * sin(_tcpOffset[3]));
1066  TCP_Offset.row(3) << 0.0 << -sin(_tcpOffset[3]) << cos(_tcpOffset[3]) << (_tcpOffset[1] * sin(_tcpOffset[3]) - _tcpOffset[2] * cos(_tcpOffset[3]));
1067  TCP_Offset.row(4) << 0.0 << 0.0 << 0.0 << 1.0;
1068  Pos = Pos * TCP_Offset;
1069  // matrix to pose
1070  std::vector<double> flangepose;
1071  flangepose.push_back(Pos(1,4));
1072  flangepose.push_back(Pos(2,4));
1073  flangepose.push_back(Pos(3,4));
1074  ColumnVector eul = ieulzxz(Pos);
1075  if (_type != K_6M180) {
1076  flangepose.push_back(eul(1));
1077  } else {
1078  // calculate phi from X and Y with K_6M180
1079  // own impl. of atan w/ 2 args for K4D compatibility --JHA
1080  double phi_calc = atan1(Pos(1,4),Pos(2,4))+mPi/2.0;
1081  if (Pos(1,4)*eul(1) < 0) {
1082  // X and given phi different sign -> tool point inwards
1083  phi_calc += mPi;
1084  }
1085  // bring phi in range [-mPi,mPi)
1086  phi_calc = fmod(phi_calc+mPi,2.0*mPi)-mPi;
1087  // bring phi in range (-mPi,mPi]
1088  if(phi_calc==-mPi) phi_calc+=2.0*mPi;
1089  // store calculated phi
1090  flangepose.push_back(phi_calc);
1091  }
1092  flangepose.push_back(eul(2));
1093  flangepose.push_back(eul(3));
1094 
1095  // Copy and complete prev
1096  std::vector<double> prev1conf;
1097  for(int i = 0; i < _dof; ++i){
1098  if (i == _dom) {
1099  // immobile = 1: _dom == _dof - 1
1100  prev1conf.push_back(_thetaimmobile);
1101  } else {
1102  prev1conf.push_back(prev.at(i));
1103  }
1104  }
1105 
1106  std::vector<double> conf;
1107 
1108  // calculate inverse kinematics (roboop)
1109  int ok = invKin_bisec(flangepose, prev1conf, conf, maxBisection);
1110 
1111  // on fail try turning phi about pi (switch inward / outward) with K_6M180
1112  if (ok<0 && _type == K_6M180) {
1113  if (flangepose[3] > 0)
1114  flangepose[3] -= mPi;
1115  else
1116  flangepose[3] += mPi;
1117  ok = invKin_bisec(flangepose, prev1conf, conf, maxBisection);
1118  }
1119 
1120  // if no solution found and type is 6M robot, get analytical guess
1121  if (ok < 0 && _type >= 0) {
1122  std::vector<double> guess;
1123  ok = anaGuess(flangepose, prev1conf, guess);
1124 
1125  // if analytical guess found, calculate inverse kinematics using guess
1126  if (ok == 1) {
1127  ok = invKin_bisec(flangepose, guess, conf, maxBisection);
1128  // check solution
1129  if (checkConfig(conf, pose, tol) == false) // use effective pose!
1130  ok = -1; // wrong solution
1131  }
1132 
1133  // if no guess or solution found, get analytical guess of near pose
1134  if (ok < 0) {
1135  std::vector<double> nearpose;
1136  nearpose.push_back(flangepose.at(0) - sign(flangepose.at(0)) * 0.05);
1137  nearpose.push_back(flangepose.at(1) - sign(flangepose.at(1)) * 0.05);
1138  nearpose.push_back(flangepose.at(2) - sign(flangepose.at(2)) * 0.05);
1139  nearpose.push_back(flangepose.at(3) - sign(flangepose.at(3)) * 0.2);
1140  nearpose.push_back(flangepose.at(4) - sign(flangepose.at(4) - mPi / 2.0) * 0.2);
1141  nearpose.push_back(flangepose.at(5) - sign(flangepose.at(5)) * 0.2);
1142  ok = anaGuess(nearpose, prev1conf, guess);
1143  // if analytical guess found, calculate inverse kinematics using guess
1144  if (ok == 1) {
1145  ok = invKin_bisec(flangepose, guess, conf, maxBisection);
1146  // check solution
1147  if (checkConfig(conf, pose, tol) == false)
1148  ok = -1; // wrong solution
1149  }
1150  }
1151  }
1152 
1153  // set angle if solution found
1154  angles.clear();
1155  if (ok == 1) {
1156  for (int i = 0; i < _dom; ++i)
1157  angles.push_back(conf.at(i));
1158  } else {
1159  // set prev if no solution found
1160  for (int i = 0; i < _dom; ++i)
1161  angles.push_back(prev.at(i));
1162  }
1163 
1164  return ok;
1165 }
1166 
1167 
1168 
1169 
1170 
int _rotDir[MaxDof]
Rotation direction.
Definition: kinematics.h:197
const double Link_length_90A_G[]
Definition: kinematics.h:161
virtual bool setLinkLength(const std::vector< double > aLengths)=0
set link length
const double Link_length_180[]
Definition: kinematics.h:162
const double Angle_range_90A_F[]
Definition: kinematics.h:153
virtual bool setAngStop(const std::vector< double > aAngStop)=0
set angle stop
FloatVector FloatVector FloatVector int typeNr
FloatVector * prev
const double Link_length_90B_G[]
Definition: kinematics.h:164
const double Angle_range_180[]
Definition: kinematics.h:155
int setEncOff(std::vector< int > encOffset)
This sets the encoder offsets.
Definition: kinematics.cpp:422
int getImmob()
Get the immobile flag of the last joint.
Definition: kinematics.cpp:607
int sign(int value)
Definition: kinematics.cpp:27
const int Rotation_direction[]
Rotation direction.
Definition: kinematics.h:171
virtual bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)=0
const Real Katana6M90A_G_data[]
Definition: kinematics.h:113
int setAngRan(std::vector< double > angleRange)
This sets the angle range.
Definition: kinematics.cpp:510
const int Dof_90
Definition: kinematics.h:74
double _tcpOffset[4]
TCP offset.
Definition: kinematics.h:220
const int MaxDof
Maximum degree of freedom.
int getAngStop(std::vector< double > &angleStop)
Get angle stop.
Definition: kinematics.cpp:666
int rad2enc(std::vector< double > angles, std::vector< int > &encoders)
Converts angles to encoders.
Definition: kinematics.cpp:826
const int Encoder_offset[]
Encoder offset.
Definition: kinematics.h:169
int setRotDir(std::vector< int > rotDir)
This sets the rotation direction.
Definition: kinematics.cpp:434
Matrix _data
Definition: kinematics.h:187
const double Angle_offset_90A_F[]
Definition: kinematics.h:145
Implements the kinematics for the Katana6M180.
int getAngRan(std::vector< double > &angleRange)
Get the angle range.
Definition: kinematics.cpp:655
FloatVector * pose
const Real Katana6M90A_F_data[]
Definition: kinematics.h:105
~KinematicsLib()
Destructor.
Definition: kinematics.cpp:21
const double Angle_range_90B_G[]
Definition: kinematics.h:157
const Real Katana6M90B_G_data[]
Definition: kinematics.h:136
int initDofMat(int dof)
Definition: kinematics.cpp:95
int setAngleMinMax()
Definition: kinematics.cpp:78
FloatVector * angleK4D
const double Link_length_90B_F[]
Definition: kinematics.h:163
int getEncOff(std::vector< int > &encOffset)
Get the encoder offsets.
Definition: kinematics.cpp:622
Implements the kinematics for the Katana6M90T.
const int Encoder_per_cycle[]
Encoder per cycle.
Definition: kinematics.h:167
int _dom
Degree of mobility.
Definition: kinematics.h:191
double _angleMin[MaxDof]
Angle min.
Definition: kinematics.h:205
bool _angRanInit
Angle range.
Definition: kinematics.h:202
Base Class for the kinematics implementations.
int setMDH(std::vector< double > theta, std::vector< double > d, std::vector< double > a, std::vector< double > alpha, int typeNr=-1)
This sets the modified Denavit-Hartenberg parameters.
Definition: kinematics.cpp:323
FloatVector * d
ReturnMatrix inv_kin(const Matrix &Tobj, const int mj=0)
Overload inv_kin function.
Definition: invkine.cpp:617
int invKin(std::vector< double > pose, std::vector< double > prev, std::vector< double > &angle)
Definition: kinematics.cpp:897
int getAngOff(std::vector< double > &angleOffset)
Get the angle offsets.
Definition: kinematics.cpp:644
int _encoderOffset[MaxDof]
Encoder offset.
Definition: kinematics.h:195
void set_q(const ColumnVector &q)
Set the joint position vector.
Definition: robot.cpp:1137
int getDOF()
Get the degree of freedom.
Definition: kinematics.cpp:581
FloatVector * angleMDH
#define KINLIB_VERSION_MINOR
Definition: kinematics.h:31
double _thetaimmobile
Definition: kinematics.h:214
const Real Katana6M90B_F_data[]
Definition: kinematics.h:128
Modified DH notation robot class.
Definition: robot.h:389
FloatVector FloatVector int maxBisection
const int Dof_180
Definition: kinematics.h:75
_T atan1(_T in1, _T in2)
Definition: kinematics.h:36
int getDOM()
Get the degree of mobility.
Definition: kinematics.cpp:585
ReturnMatrix ieulzxz(const Matrix &R)
Obtain Roll, Pitch and Yaw from a rotation matrix.
Definition: homogen.cpp:294
ColumnVector q0
Definition: demo.cpp:229
const double Angle_range_90B_F[]
Definition: kinematics.h:156
#define KINLIB_VERSION_MAJOR
Definition: kinematics.h:30
int anaGuess(std::vector< double > pose, std::vector< double > prev, std::vector< double > &angle)
Definition: kinematics.cpp:975
int _epc[MaxDof]
Encoder per cycle.
Definition: kinematics.h:193
int init()
This initializes the kinematics.
Definition: kinematics.cpp:730
int getAngMin(std::vector< double > &angleMin)
Get angle min.
Definition: kinematics.cpp:684
int invKin_bisec(std::vector< double > pose, std::vector< double > prev, std::vector< double > &conf, int maxBisection)
Definition: kinematics.cpp:940
The usual rectangular matrix.
Definition: newmat.h:625
int setAngOff(std::vector< double > angleOffset)
This sets the angle offsets.
Definition: kinematics.cpp:450
bool _matrixInit
Matrices for mDH, angle range and immobile data.
Definition: kinematics.h:186
mRobot _robot
Roboop robot class.
Definition: kinematics.h:211
const double Angle_offset_180[]
Definition: kinematics.h:147
int _dof
Degree of freedom.
Definition: kinematics.h:189
#define max(a, b)
Definition: kmlExt.cpp:32
int mDH2K4DAng(std::vector< double > angleMDH, std::vector< double > &angleK4D)
This converts angles from mDH to Katana4D convention.
Definition: kinematics.cpp:776
int getType()
Get the robot type.
Definition: kinematics.cpp:573
FloatVector FloatVector * a
FloatVector * angle
double _angleOffset[MaxDof]
Definition: kinematics.h:200
const double Angle_offset_90B_F[]
Definition: kinematics.h:148
bool checkConfig(std::vector< double > config, std::vector< double > pose, double tol=0.00001)
int setImmob(int immobile)
This sets the immobile flag of the last joint.
Definition: kinematics.cpp:393
const double Angle_range_90A_G[]
Definition: kinematics.h:154
bool _angOffInit
Angle offset.
Definition: kinematics.h:199
double _angleRange[MaxDof]
Definition: kinematics.h:203
void kine(Matrix &Rot, ColumnVector &pos) const
Direct kinematics at end effector.
Definition: kinemat.cpp:92
const double LENGTH_MULTIPLIER
Definition: kinematics.h:70
int getTcpOff(std::vector< double > &tcpOffset)
Get the tcp offset.
Definition: kinematics.cpp:710
FloatVector FloatVector FloatVector * alpha
KinematicsLib()
Constructor.
Definition: kinematics.cpp:11
int inverseKinematics(std::vector< double > pose, std::vector< double > prev, std::vector< double > &angles, int maxBisection=0)
Calculates the inverse kinematics.
Implements the kinematics for the Katana6M90G.
int setLinkLen(std::vector< double > links)
This sets the link length parameters (part of mDH-parameters)
Definition: kinematics.cpp:355
AnaGuess::Kinematics * _anaGuess
Analytical guess.
Definition: kinematics.h:216
int getVersion(std::vector< int > &version)
Get the version number of the library.
Definition: kinematics.cpp:721
bool _initialized
Kinematic initialized.
Definition: kinematics.h:218
int setTcpOff(std::vector< double > tcpOffset)
This sets the tcp offset.
Definition: kinematics.cpp:561
const Real Katana6M180_data[]
Definition: kinematics.h:121
int getEPC(std::vector< int > &epc)
Get the encoders per cycle.
Definition: kinematics.cpp:611
int K4D2mDHAng(std::vector< double > angleK4D, std::vector< double > &angleMDH)
This converts angles from Katana4D to mDH convention.
Definition: kinematics.cpp:740
ReturnMatrix eulzxz(const ColumnVector &a)
Euler ZXZ rotation.
Definition: homogen.cpp:211
int directKinematics(std::vector< double > angles, std::vector< double > &pose)
Calculates the direct kinematics.
Definition: kinematics.cpp:840
int getRotDir(std::vector< int > &rotDir)
Get the rotation direction.
Definition: kinematics.cpp:633
int enc2rad(std::vector< int > encoders, std::vector< double > &angles)
Converts encoders to angles.
Definition: kinematics.cpp:812
int angleArrMDH2vecK4D(const double arr[], std::vector< double > *vec)
Definition: kinematics.cpp:104
#define KINLIB_VERSION_REVISION
Definition: kinematics.h:32
Column vector.
Definition: newmat.h:1008
#define mPi
Definition: katana_common.h:10
int _type
Robot Type.
Definition: kinematics.h:184
int setType(int type)
This sets the robot type.
Definition: kinematics.cpp:118
int _immobile
Last joint is immobile (eg. K_6M90A_G, K_6M90B_G), _dom = _dof - 1.
Definition: kinematics.h:213
const double Angle_offset_90B_G[]
Definition: kinematics.h:149
double _angleMax[MaxDof]
Angle max.
Definition: kinematics.h:207
int getMaxDOF()
Get the maximum degree of freedom.
Definition: kinematics.cpp:577
int getAngMax(std::vector< double > &angleMax)
Get angle max.
Definition: kinematics.cpp:697
int setEPC(std::vector< int > epc)
This sets the encoders per cycle.
Definition: kinematics.cpp:410
std::vector< int > encoders
Definition: kni_wrapper.cpp:29
GetSubMatrix row(int) const
Definition: submat.cpp:49
double _linkLength[4]
Link length (for defined types)
Definition: kinematics.h:209
virtual bool setAngOff(const std::vector< double > aAngOff)=0
set angle offset
const double Link_length_90A_F[]
Definition: kinematics.h:160
const double Angle_offset_90A_G[]
Definition: kinematics.h:146
int getMDH(std::vector< double > &theta, std::vector< double > &d, std::vector< double > &a, std::vector< double > &alpha)
Get the modified Denavit-Hartenberg parameters.
Definition: kinematics.cpp:589
int initializeMembers()
Definition: kinematics.cpp:63


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