Visual Servoing Platform  version 3.3.1 under development (2020-12-02)
vpAfma6.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for the Irisa's Afma6 robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
47 #include <iostream>
48 #include <sstream>
49 
50 #include <visp3/core/vpCameraParameters.h>
51 #include <visp3/core/vpDebug.h>
52 #include <visp3/core/vpRotationMatrix.h>
53 #include <visp3/core/vpRxyzVector.h>
54 #include <visp3/core/vpTranslationVector.h>
55 #include <visp3/core/vpVelocityTwistMatrix.h>
56 #include <visp3/core/vpXmlParserCamera.h>
57 #include <visp3/robot/vpAfma6.h>
58 #include <visp3/robot/vpRobotException.h>
59 
60 /* ----------------------------------------------------------------------- */
61 /* --- STATIC ------------------------------------------------------------ */
62 /* ---------------------------------------------------------------------- */
63 
64 static const char *opt_Afma6[] = {"JOINT_MAX", "JOINT_MIN", "LONG_56", "COUPL_56",
65  "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL};
66 
67 #ifdef VISP_HAVE_AFMA6_DATA
68 const std::string vpAfma6::CONST_AFMA6_FILENAME =
69  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf");
70 
72  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf");
73 
75  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf");
76 
78  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf");
79 
81  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf");
82 
84  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf");
85 
87  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf");
88 
90  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf");
91 
93  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf");
94 
96  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf");
97 
99  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf");
100 
101 const std::string vpAfma6::CONST_CAMERA_AFMA6_FILENAME =
102  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml");
103 
104 #endif // VISP_HAVE_AFMA6_DATA
105 
106 const char *const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
107 const char *const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
108 const char *const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
109 const char *const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
110 const char *const vpAfma6::CONST_INTEL_D435_CAMERA_NAME = "Intel-D435";
111 
113 
114 const unsigned int vpAfma6::njoint = 6;
115 
122  : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(vpAfma6::defaultTool),
123  projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
124 {
125  // Set the default parameters in case of the config files are not available.
126 
127  //
128  // Geometric model constant parameters
129  //
130  // coupling between join 5 and 6
131  this->_coupl_56 = 0.009091;
132  // distance between join 5 and 6
133  this->_long_56 = -0.06924;
134  // Camera extrinsic parameters: effector to camera frame
135  this->_eMc.eye(); // Default values are initialized ...
136  // ... in init (vpAfma6::vpAfma6ToolType tool,
137  // vpCameraParameters::vpCameraParametersProjType projModel)
138  // Maximal value of the joints
139  this->_joint_max[0] = 0.7001;
140  this->_joint_max[1] = 0.5201;
141  this->_joint_max[2] = 0.4601;
142  this->_joint_max[3] = 2.7301;
143  this->_joint_max[4] = 2.4801;
144  this->_joint_max[5] = 1.5901;
145  // Minimal value of the joints
146  this->_joint_min[0] = -0.6501;
147  this->_joint_min[1] = -0.6001;
148  this->_joint_min[2] = -0.5001;
149  this->_joint_min[3] = -2.7301;
150  this->_joint_min[4] = -0.1001;
151  this->_joint_min[5] = -1.5901;
152 
153  init();
154 }
155 
160 void vpAfma6::init(void)
161 {
162  this->init(vpAfma6::defaultTool);
163  return;
164 }
165 
178 void vpAfma6::init(const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters)
179 {
180  this->parseConfigFile(camera_extrinsic_parameters);
181 
182  this->parseConfigFile(camera_intrinsic_parameters);
183 }
184 
215 void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
216 {
217  this->setToolType(tool);
218  this->parseConfigFile(filename.c_str());
219 }
220 
230 void vpAfma6::init(const std::string &camera_extrinsic_parameters)
231 {
232  this->parseConfigFile(camera_extrinsic_parameters);
233 }
234 
251 {
252  this->setToolType(tool);
253  this->set_eMc(eMc);
254 }
255 
274 {
275 
276  this->projModel = proj_model;
277 
278 #ifdef VISP_HAVE_AFMA6_DATA
279  // Read the robot parameters from files
280  std::string filename_eMc;
281  switch (tool) {
282  case vpAfma6::TOOL_CCMOP: {
283  switch (projModel) {
286  break;
289  break;
291  throw vpException(vpException::notImplementedError, "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
292  break;
293  }
294  break;
295  }
296  case vpAfma6::TOOL_GRIPPER: {
297  switch (projModel) {
300  break;
303  break;
305  throw vpException(vpException::notImplementedError, "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
306  break;
307  }
308  break;
309  }
310  case vpAfma6::TOOL_VACUUM: {
311  switch (projModel) {
314  break;
317  break;
319  throw vpException(vpException::notImplementedError, "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
320  break;
321  }
322  break;
323  }
325  switch (projModel) {
328  break;
331  break;
333  throw vpException(vpException::notImplementedError, "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
334  break;
335  }
336  break;
337  }
339  switch (projModel) {
342  break;
345  break;
347  throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
348  break;
349  }
350  break;
351  }
352  default: {
353  vpERROR_TRACE("This error should not occur!");
354  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
355  // "que les specs de la classe ont ete modifiee, "
356  // "et que le code n'a pas ete mis a jour "
357  // "correctement.");
358  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
359  // "vpAfma6::vpAfma6ToolType, et controlez que "
360  // "tous les cas ont ete pris en compte dans la "
361  // "fonction init(camera).");
362  break;
363  }
364  }
365 
366  this->init(vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
367 
368 #else // VISP_HAVE_AFMA6_DATA
369 
370  // Use here default values of the robot constant parameters.
371  switch (tool) {
372  case vpAfma6::TOOL_CCMOP: {
373  switch (projModel) {
375  _erc[0] = vpMath::rad(164.35); // rx
376  _erc[1] = vpMath::rad(89.64); // ry
377  _erc[2] = vpMath::rad(-73.05); // rz
378  _etc[0] = 0.0117; // tx
379  _etc[1] = 0.0033; // ty
380  _etc[2] = 0.2272; // tz
381  break;
383  _erc[0] = vpMath::rad(33.54); // rx
384  _erc[1] = vpMath::rad(89.34); // ry
385  _erc[2] = vpMath::rad(57.83); // rz
386  _etc[0] = 0.0373; // tx
387  _etc[1] = 0.0024; // ty
388  _etc[2] = 0.2286; // tz
389  break;
391  throw vpException(vpException::notImplementedError, "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
392  break;
393  }
394  break;
395  }
396  case vpAfma6::TOOL_GRIPPER: {
397  switch (projModel) {
399  _erc[0] = vpMath::rad(88.33); // rx
400  _erc[1] = vpMath::rad(72.07); // ry
401  _erc[2] = vpMath::rad(2.53); // rz
402  _etc[0] = 0.0783; // tx
403  _etc[1] = 0.1234; // ty
404  _etc[2] = 0.1638; // tz
405  break;
407  _erc[0] = vpMath::rad(86.69); // rx
408  _erc[1] = vpMath::rad(71.93); // ry
409  _erc[2] = vpMath::rad(4.17); // rz
410  _etc[0] = 0.1034; // tx
411  _etc[1] = 0.1142; // ty
412  _etc[2] = 0.1642; // tz
413  break;
415  throw vpException(vpException::notImplementedError, "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
416  break;
417  }
418  break;
419  }
420  case vpAfma6::TOOL_VACUUM: {
421  switch (projModel) {
423  _erc[0] = vpMath::rad(90.40); // rx
424  _erc[1] = vpMath::rad(75.11); // ry
425  _erc[2] = vpMath::rad(0.18); // rz
426  _etc[0] = 0.0038; // tx
427  _etc[1] = 0.1281; // ty
428  _etc[2] = 0.1658; // tz
429  break;
431  _erc[0] = vpMath::rad(91.61); // rx
432  _erc[1] = vpMath::rad(76.17); // ry
433  _erc[2] = vpMath::rad(-0.98); // rz
434  _etc[0] = 0.0815; // tx
435  _etc[1] = 0.1162; // ty
436  _etc[2] = 0.1658; // tz
437  break;
439  throw vpException(vpException::notImplementedError, "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
440  break;
441  }
442  break;
443  }
445  switch (projModel) {
447  _erc[0] = vpMath::rad(-71.41); // rx
448  _erc[1] = vpMath::rad(89.49); // ry
449  _erc[2] = vpMath::rad(162.07); // rz
450  _etc[0] = 0.0038; // tx
451  _etc[1] = 0.1281; // ty
452  _etc[2] = 0.1658; // tz
453  break;
455  _erc[0] = vpMath::rad(-52.79); // rx
456  _erc[1] = vpMath::rad(89.55); // ry
457  _erc[2] = vpMath::rad(143.34); // rz
458  _etc[0] = 0.0693; // tx
459  _etc[1] = -0.0297; // ty
460  _etc[2] = 0.1357; // tz
461  break;
463  throw vpException(vpException::notImplementedError, "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
464  break;
465  }
466  break;
467  }
470  switch (projModel) {
473  // set eMc to identity
474  _erc[0] = 0; // rx
475  _erc[1] = 0; // ry
476  _erc[2] = 0; // rz
477  _etc[0] = 0; // tx
478  _etc[1] = 0; // ty
479  _etc[2] = 0; // tz
480  break;
482  throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
483  break;
484  }
485  break;
486  }
487  }
488  vpRotationMatrix eRc(_erc);
489  this->_eMc.buildFrom(_etc, eRc);
490 #endif // VISP_HAVE_AFMA6_DATA
491 
492  setToolType(tool);
493  return;
494 }
495 
521 {
523  fMc = get_fMc(q);
524 
525  return fMc;
526 }
527 
600 int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest,
601  const bool &verbose) const
602 {
604  double q_[2][6], d[2], t;
605  int ok[2];
606  double cord[6];
607 
608  int nbsol = 0;
609 
610  if (q.getRows() != njoint)
611  q.resize(6);
612 
613  // for(unsigned int i=0;i<3;i++) {
614  // fMe[i][3] = fMc[i][3];
615  // for(int j=0;j<3;j++) {
616  // fMe[i][j] = 0.0;
617  // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
618  // fMe[i][3] -= fMe[i][j]*rpi[j][3];
619  // }
620  // }
621 
622  // std::cout << "\n\nfMc: " << fMc;
623  // std::cout << "\n\neMc: " << _eMc;
624 
625  fMe = fMc * this->_eMc.inverse();
626  // std::cout << "\n\nfMe: " << fMe;
627 
628  if (fMe[2][2] >= .99999f) {
629  vpTRACE("singularity\n");
630  q_[0][4] = q_[1][4] = M_PI / 2.f;
631  t = atan2(fMe[0][0], fMe[0][1]);
632  q_[1][3] = q_[0][3] = q[3];
633  q_[1][5] = q_[0][5] = t - q_[0][3];
634 
635  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
636  /* -> a cause du couplage 4/5 */
637  {
638  q_[1][5] -= vpMath::rad(10);
639  q_[1][3] += vpMath::rad(10);
640  }
641  while (q_[1][5] <= this->_joint_min[5]) {
642  q_[1][5] += vpMath::rad(10);
643  q_[1][3] -= vpMath::rad(10);
644  }
645  } else if (fMe[2][2] <= -.99999) {
646  vpTRACE("singularity\n");
647  q_[0][4] = q_[1][4] = -M_PI / 2;
648  t = atan2(fMe[1][1], fMe[1][0]);
649  q_[1][3] = q_[0][3] = q[3];
650  q_[1][5] = q_[0][5] = q_[0][3] - t;
651  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
652  /* -> a cause du couplage 4/5 */
653  {
654  q_[1][5] -= vpMath::rad(10);
655  q_[1][3] -= vpMath::rad(10);
656  }
657  while (q_[1][5] <= this->_joint_min[5]) {
658  q_[1][5] += vpMath::rad(10);
659  q_[1][3] += vpMath::rad(10);
660  }
661  } else {
662  q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
663  if (q_[0][3] >= 0.0)
664  q_[1][3] = q_[0][3] - M_PI;
665  else
666  q_[1][3] = q_[0][3] + M_PI;
667 
668  q_[0][4] = asin(fMe[2][2]);
669  if (q_[0][4] >= 0.0)
670  q_[1][4] = M_PI - q_[0][4];
671  else
672  q_[1][4] = -M_PI - q_[0][4];
673 
674  q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
675  if (q_[0][5] >= 0.0)
676  q_[1][5] = q_[0][5] - M_PI;
677  else
678  q_[1][5] = q_[0][5] + M_PI;
679  }
680  q_[0][0] = fMe[0][3] - this->_long_56 * cos(q_[0][3]);
681  q_[1][0] = fMe[0][3] - this->_long_56 * cos(q_[1][3]);
682  q_[0][1] = fMe[1][3] - this->_long_56 * sin(q_[0][3]);
683  q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]);
684  q_[0][2] = q_[1][2] = fMe[2][3];
685 
686  /* prise en compte du couplage axes 5/6 */
687  q_[0][5] += this->_coupl_56 * q_[0][4];
688  q_[1][5] += this->_coupl_56 * q_[1][4];
689 
690  for (int j = 0; j < 2; j++) {
691  ok[j] = 1;
692  // test is position is reachable
693  for (unsigned int i = 0; i < 6; i++) {
694  if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
695  if (verbose) {
696  if (i < 3)
697  std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < "
698  << this->_joint_max[i] << std::endl;
699  else
700  std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < "
701  << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
702  }
703  ok[j] = 0;
704  }
705  }
706  }
707  if (ok[0] == 0) {
708  if (ok[1] == 0) {
709  std::cout << "No solution..." << std::endl;
710  nbsol = 0;
711  return nbsol;
712  } else if (ok[1] == 1) {
713  for (unsigned int i = 0; i < 6; i++)
714  cord[i] = q_[1][i];
715  nbsol = 1;
716  }
717  } else {
718  if (ok[1] == 0) {
719  for (unsigned int i = 0; i < 6; i++)
720  cord[i] = q_[0][i];
721  nbsol = 1;
722  } else {
723  nbsol = 2;
724  // vpTRACE("2 solutions\n");
725  for (int j = 0; j < 2; j++) {
726  d[j] = 0.0;
727  for (unsigned int i = 3; i < 6; i++)
728  d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
729  }
730  if (nearest == true) {
731  if (d[0] <= d[1])
732  for (unsigned int i = 0; i < 6; i++)
733  cord[i] = q_[0][i];
734  else
735  for (unsigned int i = 0; i < 6; i++)
736  cord[i] = q_[1][i];
737  } else {
738  if (d[0] <= d[1])
739  for (unsigned int i = 0; i < 6; i++)
740  cord[i] = q_[1][i];
741  else
742  for (unsigned int i = 0; i < 6; i++)
743  cord[i] = q_[0][i];
744  }
745  }
746  }
747  for (unsigned int i = 0; i < 6; i++)
748  q[i] = cord[i];
749 
750  return nbsol;
751 }
752 
776 {
778  get_fMc(q, fMc);
779 
780  return fMc;
781 }
782 
803 {
804 
805  // Compute the direct geometric model: fMe = transformation between
806  // fix and end effector frame.
808 
809  get_fMe(q, fMe);
810 
811  fMc = fMe * this->_eMc;
812 
813  return;
814 }
815 
836 {
837  double q0 = q[0]; // meter
838  double q1 = q[1]; // meter
839  double q2 = q[2]; // meter
840 
841  /* Decouplage liaisons 2 et 3. */
842  double q5 = q[5] - this->_coupl_56 * q[4];
843 
844  double c1 = cos(q[3]);
845  double s1 = sin(q[3]);
846  double c2 = cos(q[4]);
847  double s2 = sin(q[4]);
848  double c3 = cos(q5);
849  double s3 = sin(q5);
850 
851  // Compute the direct geometric model: fMe = transformation betwee
852  // fix and end effector frame.
853  fMe[0][0] = s1 * s2 * c3 + c1 * s3;
854  fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
855  fMe[0][2] = -s1 * c2;
856  fMe[0][3] = q0 + this->_long_56 * c1;
857 
858  fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
859  fMe[1][1] = c1 * s2 * s3 + s1 * c3;
860  fMe[1][2] = c1 * c2;
861  fMe[1][3] = q1 + this->_long_56 * s1;
862 
863  fMe[2][0] = c2 * c3;
864  fMe[2][1] = -c2 * s3;
865  fMe[2][2] = s2;
866  fMe[2][3] = q2;
867 
868  fMe[3][0] = 0;
869  fMe[3][1] = 0;
870  fMe[3][2] = 0;
871  fMe[3][3] = 1;
872 
873  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
874 
875  return;
876 }
877 
888 void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
899 vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
900 
911 {
913  get_cMe(cMe);
914 
915  cVe.buildFrom(cMe);
916 
917  return;
918 }
919 
932 void vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
933 {
934 
935  eJe.resize(6, 6);
936 
937  double s4, c4, s5, c5, s6, c6;
938 
939  s4 = sin(q[3]);
940  c4 = cos(q[3]);
941  s5 = sin(q[4]);
942  c5 = cos(q[4]);
943  s6 = sin(q[5] - this->_coupl_56 * q[4]);
944  c6 = cos(q[5] - this->_coupl_56 * q[4]);
945 
946  eJe = 0;
947  eJe[0][0] = s4 * s5 * c6 + c4 * s6;
948  eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
949  eJe[0][2] = c5 * c6;
950  eJe[0][3] = -this->_long_56 * s5 * c6;
951 
952  eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
953  eJe[1][1] = c4 * s5 * s6 + s4 * c6;
954  eJe[1][2] = -c5 * s6;
955  eJe[1][3] = this->_long_56 * s5 * s6;
956 
957  eJe[2][0] = -s4 * c5;
958  eJe[2][1] = c4 * c5;
959  eJe[2][2] = s5;
960  eJe[2][3] = this->_long_56 * c5;
961 
962  eJe[3][3] = c5 * c6;
963  eJe[3][4] = s6;
964 
965  eJe[4][3] = -c5 * s6;
966  eJe[4][4] = c6;
967 
968  eJe[5][3] = s5;
969  eJe[5][4] = -this->_coupl_56;
970  eJe[5][5] = 1;
971 
972  return;
973 }
974 
1002 void vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1003 {
1004 
1005  fJe.resize(6, 6);
1006 
1007  // block superieur gauche
1008  fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1009 
1010  double s4 = sin(q[3]);
1011  double c4 = cos(q[3]);
1012 
1013  // block superieur droit
1014  fJe[0][3] = -this->_long_56 * s4;
1015  fJe[1][3] = this->_long_56 * c4;
1016 
1017  double s5 = sin(q[4]);
1018  double c5 = cos(q[4]);
1019  // block inferieur droit
1020  fJe[3][4] = c4;
1021  fJe[3][5] = -s4 * c5;
1022  fJe[4][4] = s4;
1023  fJe[4][5] = c4 * c5;
1024  fJe[5][3] = 1;
1025  fJe[5][5] = s5;
1026 
1027  // coupling between joint 5 and 6
1028  fJe[3][4] += this->_coupl_56 * s4 * c5;
1029  fJe[4][4] += -this->_coupl_56 * c4 * c5;
1030  fJe[5][4] += -this->_coupl_56 * s5;
1031 
1032  return;
1033 }
1034 
1044 {
1045  vpColVector qmin(6);
1046  for (unsigned int i = 0; i < 6; i++)
1047  qmin[i] = this->_joint_min[i];
1048  return qmin;
1049 }
1050 
1060 {
1061  vpColVector qmax(6);
1062  for (unsigned int i = 0; i < 6; i++)
1063  qmax[i] = this->_joint_max[i];
1064  return qmax;
1065 }
1066 
1073 double vpAfma6::getCoupl56() const { return _coupl_56; }
1074 
1081 double vpAfma6::getLong56() const { return _long_56; }
1082 
1093 void vpAfma6::parseConfigFile(const std::string &filename)
1094 {
1095  vpRxyzVector erc; // eMc rotation
1096  vpTranslationVector etc; // eMc translation
1097 
1098  std::ifstream fdconfig(filename.c_str(), std::ios::in);
1099 
1100  if (!fdconfig.is_open()) {
1101  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
1102  filename.c_str());
1103  }
1104 
1105  std::string line;
1106  int lineNum = 0;
1107  bool get_erc = false;
1108  bool get_etc = false;
1109  int code;
1110 
1111  while (std::getline(fdconfig, line)) {
1112  lineNum++;
1113  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
1114  continue;
1115  }
1116  std::istringstream ss(line);
1117  std::string key;
1118  ss >> key;
1119 
1120  for (code = 0; NULL != opt_Afma6[code]; ++code) {
1121  if (key.compare(opt_Afma6[code]) == 0) {
1122  break;
1123  }
1124  }
1125 
1126  switch (code) {
1127  case 0:
1128  ss >> this->_joint_max[0] >> this->_joint_max[1] >> this->_joint_max[2] >> this->_joint_max[3] >>
1129  this->_joint_max[4] >> this->_joint_max[5];
1130  break;
1131 
1132  case 1:
1133  ss >> this->_joint_min[0] >> this->_joint_min[1] >> this->_joint_min[2] >> this->_joint_min[3] >>
1134  this->_joint_min[4] >> this->_joint_min[5];
1135  break;
1136 
1137  case 2:
1138  ss >> this->_long_56;
1139  break;
1140 
1141  case 3:
1142  ss >> this->_coupl_56;
1143  break;
1144 
1145  case 4:
1146  break; // Nothing to do: camera name
1147 
1148  case 5:
1149  ss >> erc[0] >> erc[1] >> erc[2];
1150 
1151  // Convert rotation from degrees to radians
1152  erc = erc * M_PI / 180.0;
1153  get_erc = true;
1154  break;
1155 
1156  case 6:
1157  ss >> etc[0] >> etc[1] >> etc[2];
1158  get_etc = true;
1159  break;
1160 
1161  default:
1162  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
1163  filename.c_str(), lineNum));
1164  }
1165  }
1166 
1167  fdconfig.close();
1168 
1169  // Compute the eMc matrix from the translations and rotations
1170  if (get_etc && get_erc) {
1171  _erc = erc;
1172  _etc = etc;
1173 
1174  vpRotationMatrix eRc(_erc);
1175  this->_eMc.buildFrom(_etc, eRc);
1176  }
1177 }
1178 
1188 {
1189  this->_eMc = eMc;
1190  this->_etc = eMc.getTranslationVector();
1191 
1192  vpRotationMatrix R(eMc);
1193  this->_erc.buildFrom(R);
1194 }
1195 
1256 void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
1257  const unsigned int &image_height) const
1258 {
1259 #if defined(VISP_HAVE_AFMA6_DATA)
1260  vpXmlParserCamera parser;
1261  switch (getToolType()) {
1262  case vpAfma6::TOOL_CCMOP: {
1263  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1264  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1266  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1267  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1268  }
1269  break;
1270  }
1271  case vpAfma6::TOOL_GRIPPER: {
1272  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1273  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1275  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1276  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1277  }
1278  break;
1279  }
1280  case vpAfma6::TOOL_VACUUM: {
1281  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1282  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1284  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1285  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1286  }
1287  break;
1288  }
1290  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\"" << std::endl
1291  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1293  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1294  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1295  }
1296  break;
1297  }
1299  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1300  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1302  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1303  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1304  }
1305  break;
1306  }
1307  default: {
1308  vpERROR_TRACE("This error should not occur!");
1309  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1310  // "que les specs de la classe ont ete modifiee, "
1311  // "et que le code n'a pas ete mis a jour "
1312  // "correctement.");
1313  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1314  // "vpAfma6::vpAfma6ToolType, et controlez que "
1315  // "tous les cas ont ete pris en compte dans la "
1316  // "fonction init(camera).");
1317  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1318  }
1319  }
1320 #else
1321  // Set default parameters
1322  switch (getToolType()) {
1323  case vpAfma6::TOOL_CCMOP: {
1324  // Set default intrinsic camera parameters for 640x480 images
1325  if (image_width == 640 && image_height == 480) {
1326  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
1327  << std::endl;
1328  switch (this->projModel) {
1330  cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1331  break;
1333  cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1334  break;
1336  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1337  break;
1338  }
1339  } else {
1340  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1341  "resolution");
1342  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1343  }
1344  break;
1345  }
1346  case vpAfma6::TOOL_GRIPPER: {
1347  // Set default intrinsic camera parameters for 640x480 images
1348  if (image_width == 640 && image_height == 480) {
1349  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
1350  << std::endl;
1351  switch (this->projModel) {
1353  cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
1354  break;
1356  cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
1357  break;
1359  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1360  break;
1361  }
1362  } else {
1363  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1364  "resolution");
1365  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1366  }
1367  break;
1368  }
1369  case vpAfma6::TOOL_VACUUM: {
1370  // Set default intrinsic camera parameters for 640x480 images
1371  if (image_width == 640 && image_height == 480) {
1372  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\""
1373  << std::endl;
1374  switch (this->projModel) {
1376  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1377  break;
1379  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1380  break;
1382  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1383  break;
1384  }
1385  } else {
1386  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1387  "resolution");
1388  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1389  }
1390  break;
1391  }
1393  // Set default intrinsic camera parameters for 640x480 images
1394  if (image_width == 640 && image_height == 480) {
1395  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\""
1396  << std::endl;
1397  switch (this->projModel) {
1399  cam.initPersProjWithoutDistortion(605.4, 605.6, 328.6, 241.0);
1400  break;
1402  cam.initPersProjWithDistortion(611.8, 612.6, 327.8, 241.7, 0.0436, -0.04265);
1403  break;
1405  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1406  break;
1407  }
1408  } else {
1409  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1410  "resolution");
1411  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1412  }
1413  break;
1414  }
1416  // Set default intrinsic camera parameters for 640x480 images
1417  if (image_width == 640 && image_height == 480) {
1418  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\""
1419  << std::endl;
1420  switch (this->projModel) {
1422  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1423  break;
1425  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1426  break;
1428  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1429  break;
1430  }
1431  } else {
1432  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1433  "resolution");
1434  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1435  }
1436  break;
1437  }
1438  default:
1439  vpERROR_TRACE("This error should not occur!");
1440  break;
1441  }
1442 #endif
1443  return;
1444 }
1445 
1489 {
1490  getCameraParameters(cam, I.getWidth(), I.getHeight());
1491 }
1536 {
1537  getCameraParameters(cam, I.getWidth(), I.getHeight());
1538 }
1539 
1549 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6)
1550 {
1551  vpRotationMatrix eRc;
1552  afma6._eMc.extract(eRc);
1553  vpRxyzVector rxyz(eRc);
1554 
1555  os << "Joint Max:" << std::endl
1556  << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t"
1557  << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl
1558 
1559  << "Joint Min: " << std::endl
1560  << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t"
1561  << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl
1562 
1563  << "Long 5-6: " << std::endl
1564  << "\t" << afma6._long_56 << "\t" << std::endl
1565 
1566  << "Coupling 5-6:" << std::endl
1567  << "\t" << afma6._coupl_56 << "\t" << std::endl
1568 
1569  << "eMc: " << std::endl
1570  << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t"
1571  << std::endl
1572  << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1573  << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1574  << "\t" << std::endl;
1575 
1576  return os;
1577 }
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:94
Modelisation of Irisa&#39;s gantry robot named Afma6.
Definition: vpAfma6.h:78
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpRxyzVector buildFrom(const vpRotationMatrix &R)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1256
vpRxyzVector _erc
Definition: vpAfma6.h:207
vpTranslationVector _etc
Definition: vpAfma6.h:206
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:194
static const std::string CONST_AFMA6_FILENAME
Definition: vpAfma6.h:85
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
Error that can be emited by the vpRobot class and its derivates.
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:92
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:88
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpColVector getJointMax() const
Definition: vpAfma6.cpp:1059
#define vpERROR_TRACE
Definition: vpDebug.h:393
void parseConfigFile(const std::string &filename)
Definition: vpAfma6.cpp:1093
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:87
double _coupl_56
Definition: vpAfma6.h:201
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:600
static const std::string CONST_CAMERA_AFMA6_FILENAME
Definition: vpAfma6.h:96
error that can be emited by ViSP classes.
Definition: vpException.h:71
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:910
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:126
unsigned int getRows() const
Definition: vpArray2D.h:289
vpHomogeneousMatrix inverse() const
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:215
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:169
void extract(vpRotationMatrix &R) const
XML parser to load and save intrinsic camera parameters.
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:775
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpAfma6 &afma6)
Definition: vpAfma6.cpp:1549
double getLong56() const
Definition: vpAfma6.cpp:1081
static const std::string CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:93
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:194
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:95
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:107
Implementation of a rotation matrix and operations on such kind of matrices.
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:136
vpHomogeneousMatrix get_eMc() const
Definition: vpAfma6.cpp:899
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:90
#define vpTRACE
Definition: vpDebug.h:416
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:91
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:932
Generic class defining intrinsic camera parameters.
vpColVector getJointMin() const
Definition: vpAfma6.cpp:1043
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma6.cpp:520
vpAfma6()
Definition: vpAfma6.cpp:121
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1002
static double rad(double deg)
Definition: vpMath.h:110
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:86
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0)
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma6.cpp:835
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1187
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpTranslationVector getTranslationVector() const
double _long_56
Definition: vpAfma6.h:202
static double deg(double rad)
Definition: vpMath.h:103
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:888
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:209
static const char *const CONST_INTEL_D435_CAMERA_NAME
Definition: vpAfma6.h:123
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
void init(void)
Definition: vpAfma6.cpp:160
double _joint_max[6]
Definition: vpAfma6.h:203
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:102
unsigned int getWidth() const
Definition: vpImage.h:246
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:89
static const char *const CONST_VACUUM_CAMERA_NAME
Definition: vpAfma6.h:112
Class that consider the case of a translation vector.
double getCoupl56() const
Definition: vpAfma6.cpp:1073
double _joint_min[6]
Definition: vpAfma6.h:204
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpAfma6.h:117