Visual Servoing Platform  version 3.5.1 under development (2022-12-01)
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;
292  "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
293  break;
294  }
295  break;
296  }
297  case vpAfma6::TOOL_GRIPPER: {
298  switch (projModel) {
301  break;
304  break;
307  "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
308  break;
309  }
310  break;
311  }
312  case vpAfma6::TOOL_VACUUM: {
313  switch (projModel) {
316  break;
319  break;
322  "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
323  break;
324  }
325  break;
326  }
328  switch (projModel) {
331  break;
334  break;
337  "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
338  break;
339  }
340  break;
341  }
343  switch (projModel) {
346  break;
349  break;
352  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
353  break;
354  }
355  break;
356  }
357  default: {
358  vpERROR_TRACE("This error should not occur!");
359  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
360  // "que les specs de la classe ont ete modifiee, "
361  // "et que le code n'a pas ete mis a jour "
362  // "correctement.");
363  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
364  // "vpAfma6::vpAfma6ToolType, et controlez que "
365  // "tous les cas ont ete pris en compte dans la "
366  // "fonction init(camera).");
367  break;
368  }
369  }
370 
371  this->init(vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
372 
373 #else // VISP_HAVE_AFMA6_DATA
374 
375  // Use here default values of the robot constant parameters.
376  switch (tool) {
377  case vpAfma6::TOOL_CCMOP: {
378  switch (projModel) {
380  _erc[0] = vpMath::rad(164.35); // rx
381  _erc[1] = vpMath::rad(89.64); // ry
382  _erc[2] = vpMath::rad(-73.05); // rz
383  _etc[0] = 0.0117; // tx
384  _etc[1] = 0.0033; // ty
385  _etc[2] = 0.2272; // tz
386  break;
388  _erc[0] = vpMath::rad(33.54); // rx
389  _erc[1] = vpMath::rad(89.34); // ry
390  _erc[2] = vpMath::rad(57.83); // rz
391  _etc[0] = 0.0373; // tx
392  _etc[1] = 0.0024; // ty
393  _etc[2] = 0.2286; // tz
394  break;
397  "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
398  break;
399  }
400  break;
401  }
402  case vpAfma6::TOOL_GRIPPER: {
403  switch (projModel) {
405  _erc[0] = vpMath::rad(88.33); // rx
406  _erc[1] = vpMath::rad(72.07); // ry
407  _erc[2] = vpMath::rad(2.53); // rz
408  _etc[0] = 0.0783; // tx
409  _etc[1] = 0.1234; // ty
410  _etc[2] = 0.1638; // tz
411  break;
413  _erc[0] = vpMath::rad(86.69); // rx
414  _erc[1] = vpMath::rad(71.93); // ry
415  _erc[2] = vpMath::rad(4.17); // rz
416  _etc[0] = 0.1034; // tx
417  _etc[1] = 0.1142; // ty
418  _etc[2] = 0.1642; // tz
419  break;
422  "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
423  break;
424  }
425  break;
426  }
427  case vpAfma6::TOOL_VACUUM: {
428  switch (projModel) {
430  _erc[0] = vpMath::rad(90.40); // rx
431  _erc[1] = vpMath::rad(75.11); // ry
432  _erc[2] = vpMath::rad(0.18); // rz
433  _etc[0] = 0.0038; // tx
434  _etc[1] = 0.1281; // ty
435  _etc[2] = 0.1658; // tz
436  break;
438  _erc[0] = vpMath::rad(91.61); // rx
439  _erc[1] = vpMath::rad(76.17); // ry
440  _erc[2] = vpMath::rad(-0.98); // rz
441  _etc[0] = 0.0815; // tx
442  _etc[1] = 0.1162; // ty
443  _etc[2] = 0.1658; // tz
444  break;
447  "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
448  break;
449  }
450  break;
451  }
453  switch (projModel) {
455  _erc[0] = vpMath::rad(-71.41); // rx
456  _erc[1] = vpMath::rad(89.49); // ry
457  _erc[2] = vpMath::rad(162.07); // rz
458  _etc[0] = 0.0038; // tx
459  _etc[1] = 0.1281; // ty
460  _etc[2] = 0.1658; // tz
461  break;
463  _erc[0] = vpMath::rad(-52.79); // rx
464  _erc[1] = vpMath::rad(89.55); // ry
465  _erc[2] = vpMath::rad(143.34); // rz
466  _etc[0] = 0.0693; // tx
467  _etc[1] = -0.0297; // ty
468  _etc[2] = 0.1357; // tz
469  break;
472  "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
473  break;
474  }
475  break;
476  }
479  switch (projModel) {
482  // set eMc to identity
483  _erc[0] = 0; // rx
484  _erc[1] = 0; // ry
485  _erc[2] = 0; // rz
486  _etc[0] = 0; // tx
487  _etc[1] = 0; // ty
488  _etc[2] = 0; // tz
489  break;
492  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
493  break;
494  }
495  break;
496  }
497  }
498  vpRotationMatrix eRc(_erc);
499  this->_eMc.buildFrom(_etc, eRc);
500 #endif // VISP_HAVE_AFMA6_DATA
501 
502  setToolType(tool);
503  return;
504 }
505 
531 {
533  fMc = get_fMc(q);
534 
535  return fMc;
536 }
537 
610 int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest,
611  const bool &verbose) const
612 {
614  double q_[2][6], d[2], t;
615  int ok[2];
616  double cord[6];
617 
618  int nbsol = 0;
619 
620  if (q.getRows() != njoint)
621  q.resize(6);
622 
623  // for(unsigned int i=0;i<3;i++) {
624  // fMe[i][3] = fMc[i][3];
625  // for(int j=0;j<3;j++) {
626  // fMe[i][j] = 0.0;
627  // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
628  // fMe[i][3] -= fMe[i][j]*rpi[j][3];
629  // }
630  // }
631 
632  // std::cout << "\n\nfMc: " << fMc;
633  // std::cout << "\n\neMc: " << _eMc;
634 
635  fMe = fMc * this->_eMc.inverse();
636  // std::cout << "\n\nfMe: " << fMe;
637 
638  if (fMe[2][2] >= .99999f) {
639  vpTRACE("singularity\n");
640  q_[0][4] = q_[1][4] = M_PI / 2.f;
641  t = atan2(fMe[0][0], fMe[0][1]);
642  q_[1][3] = q_[0][3] = q[3];
643  q_[1][5] = q_[0][5] = t - q_[0][3];
644 
645  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
646  /* -> a cause du couplage 4/5 */
647  {
648  q_[1][5] -= vpMath::rad(10);
649  q_[1][3] += vpMath::rad(10);
650  }
651  while (q_[1][5] <= this->_joint_min[5]) {
652  q_[1][5] += vpMath::rad(10);
653  q_[1][3] -= vpMath::rad(10);
654  }
655  } else if (fMe[2][2] <= -.99999) {
656  vpTRACE("singularity\n");
657  q_[0][4] = q_[1][4] = -M_PI / 2;
658  t = atan2(fMe[1][1], fMe[1][0]);
659  q_[1][3] = q_[0][3] = q[3];
660  q_[1][5] = q_[0][5] = q_[0][3] - t;
661  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
662  /* -> a cause du couplage 4/5 */
663  {
664  q_[1][5] -= vpMath::rad(10);
665  q_[1][3] -= vpMath::rad(10);
666  }
667  while (q_[1][5] <= this->_joint_min[5]) {
668  q_[1][5] += vpMath::rad(10);
669  q_[1][3] += vpMath::rad(10);
670  }
671  } else {
672  q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
673  if (q_[0][3] >= 0.0)
674  q_[1][3] = q_[0][3] - M_PI;
675  else
676  q_[1][3] = q_[0][3] + M_PI;
677 
678  q_[0][4] = asin(fMe[2][2]);
679  if (q_[0][4] >= 0.0)
680  q_[1][4] = M_PI - q_[0][4];
681  else
682  q_[1][4] = -M_PI - q_[0][4];
683 
684  q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
685  if (q_[0][5] >= 0.0)
686  q_[1][5] = q_[0][5] - M_PI;
687  else
688  q_[1][5] = q_[0][5] + M_PI;
689  }
690  q_[0][0] = fMe[0][3] - this->_long_56 * cos(q_[0][3]);
691  q_[1][0] = fMe[0][3] - this->_long_56 * cos(q_[1][3]);
692  q_[0][1] = fMe[1][3] - this->_long_56 * sin(q_[0][3]);
693  q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]);
694  q_[0][2] = q_[1][2] = fMe[2][3];
695 
696  /* prise en compte du couplage axes 5/6 */
697  q_[0][5] += this->_coupl_56 * q_[0][4];
698  q_[1][5] += this->_coupl_56 * q_[1][4];
699 
700  for (int j = 0; j < 2; j++) {
701  ok[j] = 1;
702  // test is position is reachable
703  for (unsigned int i = 0; i < 6; i++) {
704  if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
705  if (verbose) {
706  if (i < 3)
707  std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < "
708  << this->_joint_max[i] << std::endl;
709  else
710  std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < "
711  << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
712  }
713  ok[j] = 0;
714  }
715  }
716  }
717  if (ok[0] == 0) {
718  if (ok[1] == 0) {
719  std::cout << "No solution..." << std::endl;
720  nbsol = 0;
721  return nbsol;
722  } else if (ok[1] == 1) {
723  for (unsigned int i = 0; i < 6; i++)
724  cord[i] = q_[1][i];
725  nbsol = 1;
726  }
727  } else {
728  if (ok[1] == 0) {
729  for (unsigned int i = 0; i < 6; i++)
730  cord[i] = q_[0][i];
731  nbsol = 1;
732  } else {
733  nbsol = 2;
734  // vpTRACE("2 solutions\n");
735  for (int j = 0; j < 2; j++) {
736  d[j] = 0.0;
737  for (unsigned int i = 3; i < 6; i++)
738  d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
739  }
740  if (nearest == true) {
741  if (d[0] <= d[1])
742  for (unsigned int i = 0; i < 6; i++)
743  cord[i] = q_[0][i];
744  else
745  for (unsigned int i = 0; i < 6; i++)
746  cord[i] = q_[1][i];
747  } else {
748  if (d[0] <= d[1])
749  for (unsigned int i = 0; i < 6; i++)
750  cord[i] = q_[1][i];
751  else
752  for (unsigned int i = 0; i < 6; i++)
753  cord[i] = q_[0][i];
754  }
755  }
756  }
757  for (unsigned int i = 0; i < 6; i++)
758  q[i] = cord[i];
759 
760  return nbsol;
761 }
762 
786 {
788  get_fMc(q, fMc);
789 
790  return fMc;
791 }
792 
813 {
814 
815  // Compute the direct geometric model: fMe = transformation between
816  // fix and end effector frame.
818 
819  get_fMe(q, fMe);
820 
821  fMc = fMe * this->_eMc;
822 
823  return;
824 }
825 
846 {
847  double q0 = q[0]; // meter
848  double q1 = q[1]; // meter
849  double q2 = q[2]; // meter
850 
851  /* Decouplage liaisons 2 et 3. */
852  double q5 = q[5] - this->_coupl_56 * q[4];
853 
854  double c1 = cos(q[3]);
855  double s1 = sin(q[3]);
856  double c2 = cos(q[4]);
857  double s2 = sin(q[4]);
858  double c3 = cos(q5);
859  double s3 = sin(q5);
860 
861  // Compute the direct geometric model: fMe = transformation betwee
862  // fix and end effector frame.
863  fMe[0][0] = s1 * s2 * c3 + c1 * s3;
864  fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
865  fMe[0][2] = -s1 * c2;
866  fMe[0][3] = q0 + this->_long_56 * c1;
867 
868  fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
869  fMe[1][1] = c1 * s2 * s3 + s1 * c3;
870  fMe[1][2] = c1 * c2;
871  fMe[1][3] = q1 + this->_long_56 * s1;
872 
873  fMe[2][0] = c2 * c3;
874  fMe[2][1] = -c2 * s3;
875  fMe[2][2] = s2;
876  fMe[2][3] = q2;
877 
878  fMe[3][0] = 0;
879  fMe[3][1] = 0;
880  fMe[3][2] = 0;
881  fMe[3][3] = 1;
882 
883  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
884 
885  return;
886 }
887 
898 void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
909 vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
910 
921 {
923  get_cMe(cMe);
924 
925  cVe.buildFrom(cMe);
926 
927  return;
928 }
929 
942 void vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
943 {
944 
945  eJe.resize(6, 6);
946 
947  double s4, c4, s5, c5, s6, c6;
948 
949  s4 = sin(q[3]);
950  c4 = cos(q[3]);
951  s5 = sin(q[4]);
952  c5 = cos(q[4]);
953  s6 = sin(q[5] - this->_coupl_56 * q[4]);
954  c6 = cos(q[5] - this->_coupl_56 * q[4]);
955 
956  eJe = 0;
957  eJe[0][0] = s4 * s5 * c6 + c4 * s6;
958  eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
959  eJe[0][2] = c5 * c6;
960  eJe[0][3] = -this->_long_56 * s5 * c6;
961 
962  eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
963  eJe[1][1] = c4 * s5 * s6 + s4 * c6;
964  eJe[1][2] = -c5 * s6;
965  eJe[1][3] = this->_long_56 * s5 * s6;
966 
967  eJe[2][0] = -s4 * c5;
968  eJe[2][1] = c4 * c5;
969  eJe[2][2] = s5;
970  eJe[2][3] = this->_long_56 * c5;
971 
972  eJe[3][3] = c5 * c6;
973  eJe[3][4] = s6;
974 
975  eJe[4][3] = -c5 * s6;
976  eJe[4][4] = c6;
977 
978  eJe[5][3] = s5;
979  eJe[5][4] = -this->_coupl_56;
980  eJe[5][5] = 1;
981 
982  return;
983 }
984 
1012 void vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1013 {
1014 
1015  fJe.resize(6, 6);
1016 
1017  // block superieur gauche
1018  fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1019 
1020  double s4 = sin(q[3]);
1021  double c4 = cos(q[3]);
1022 
1023  // block superieur droit
1024  fJe[0][3] = -this->_long_56 * s4;
1025  fJe[1][3] = this->_long_56 * c4;
1026 
1027  double s5 = sin(q[4]);
1028  double c5 = cos(q[4]);
1029  // block inferieur droit
1030  fJe[3][4] = c4;
1031  fJe[3][5] = -s4 * c5;
1032  fJe[4][4] = s4;
1033  fJe[4][5] = c4 * c5;
1034  fJe[5][3] = 1;
1035  fJe[5][5] = s5;
1036 
1037  // coupling between joint 5 and 6
1038  fJe[3][4] += this->_coupl_56 * s4 * c5;
1039  fJe[4][4] += -this->_coupl_56 * c4 * c5;
1040  fJe[5][4] += -this->_coupl_56 * s5;
1041 
1042  return;
1043 }
1044 
1054 {
1055  vpColVector qmin(6);
1056  for (unsigned int i = 0; i < 6; i++)
1057  qmin[i] = this->_joint_min[i];
1058  return qmin;
1059 }
1060 
1070 {
1071  vpColVector qmax(6);
1072  for (unsigned int i = 0; i < 6; i++)
1073  qmax[i] = this->_joint_max[i];
1074  return qmax;
1075 }
1076 
1083 double vpAfma6::getCoupl56() const { return _coupl_56; }
1084 
1091 double vpAfma6::getLong56() const { return _long_56; }
1092 
1103 void vpAfma6::parseConfigFile(const std::string &filename)
1104 {
1105  vpRxyzVector erc; // eMc rotation
1106  vpTranslationVector etc; // eMc translation
1107 
1108  std::ifstream fdconfig(filename.c_str(), std::ios::in);
1109 
1110  if (!fdconfig.is_open()) {
1111  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
1112  filename.c_str());
1113  }
1114 
1115  std::string line;
1116  int lineNum = 0;
1117  bool get_erc = false;
1118  bool get_etc = false;
1119  int code;
1120 
1121  while (std::getline(fdconfig, line)) {
1122  lineNum++;
1123  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
1124  continue;
1125  }
1126  std::istringstream ss(line);
1127  std::string key;
1128  ss >> key;
1129 
1130  for (code = 0; NULL != opt_Afma6[code]; ++code) {
1131  if (key.compare(opt_Afma6[code]) == 0) {
1132  break;
1133  }
1134  }
1135 
1136  switch (code) {
1137  case 0:
1138  ss >> this->_joint_max[0] >> this->_joint_max[1] >> this->_joint_max[2] >> this->_joint_max[3] >>
1139  this->_joint_max[4] >> this->_joint_max[5];
1140  break;
1141 
1142  case 1:
1143  ss >> this->_joint_min[0] >> this->_joint_min[1] >> this->_joint_min[2] >> this->_joint_min[3] >>
1144  this->_joint_min[4] >> this->_joint_min[5];
1145  break;
1146 
1147  case 2:
1148  ss >> this->_long_56;
1149  break;
1150 
1151  case 3:
1152  ss >> this->_coupl_56;
1153  break;
1154 
1155  case 4:
1156  break; // Nothing to do: camera name
1157 
1158  case 5:
1159  ss >> erc[0] >> erc[1] >> erc[2];
1160 
1161  // Convert rotation from degrees to radians
1162  erc = erc * M_PI / 180.0;
1163  get_erc = true;
1164  break;
1165 
1166  case 6:
1167  ss >> etc[0] >> etc[1] >> etc[2];
1168  get_etc = true;
1169  break;
1170 
1171  default:
1172  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
1173  filename.c_str(), lineNum));
1174  }
1175  }
1176 
1177  fdconfig.close();
1178 
1179  // Compute the eMc matrix from the translations and rotations
1180  if (get_etc && get_erc) {
1181  _erc = erc;
1182  _etc = etc;
1183 
1184  vpRotationMatrix eRc(_erc);
1185  this->_eMc.buildFrom(_etc, eRc);
1186  }
1187 }
1188 
1198 {
1199  this->_eMc = eMc;
1200  this->_etc = eMc.getTranslationVector();
1201 
1202  vpRotationMatrix R(eMc);
1203  this->_erc.buildFrom(R);
1204 }
1205 
1266 void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
1267  const unsigned int &image_height) const
1268 {
1269 #if defined(VISP_HAVE_AFMA6_DATA)
1270  vpXmlParserCamera parser;
1271  switch (getToolType()) {
1272  case vpAfma6::TOOL_CCMOP: {
1273  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1274  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1276  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1277  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1278  }
1279  break;
1280  }
1281  case vpAfma6::TOOL_GRIPPER: {
1282  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1283  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1285  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1286  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1287  }
1288  break;
1289  }
1290  case vpAfma6::TOOL_VACUUM: {
1291  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1292  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1294  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1295  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1296  }
1297  break;
1298  }
1300  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\"" << std::endl
1301  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1303  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1304  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1305  }
1306  break;
1307  }
1309  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1310  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1312  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1313  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1314  }
1315  break;
1316  }
1317  default: {
1318  vpERROR_TRACE("This error should not occur!");
1319  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1320  // "que les specs de la classe ont ete modifiee, "
1321  // "et que le code n'a pas ete mis a jour "
1322  // "correctement.");
1323  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1324  // "vpAfma6::vpAfma6ToolType, et controlez que "
1325  // "tous les cas ont ete pris en compte dans la "
1326  // "fonction init(camera).");
1327  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1328  }
1329  }
1330 #else
1331  // Set default parameters
1332  switch (getToolType()) {
1333  case vpAfma6::TOOL_CCMOP: {
1334  // Set default intrinsic camera parameters for 640x480 images
1335  if (image_width == 640 && image_height == 480) {
1336  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
1337  << std::endl;
1338  switch (this->projModel) {
1340  cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1341  break;
1343  cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1344  break;
1347  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1348  break;
1349  }
1350  } else {
1351  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1352  "resolution");
1353  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1354  }
1355  break;
1356  }
1357  case vpAfma6::TOOL_GRIPPER: {
1358  // Set default intrinsic camera parameters for 640x480 images
1359  if (image_width == 640 && image_height == 480) {
1360  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
1361  << std::endl;
1362  switch (this->projModel) {
1364  cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
1365  break;
1367  cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
1368  break;
1371  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1372  break;
1373  }
1374  } else {
1375  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1376  "resolution");
1377  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1378  }
1379  break;
1380  }
1381  case vpAfma6::TOOL_VACUUM: {
1382  // Set default intrinsic camera parameters for 640x480 images
1383  if (image_width == 640 && image_height == 480) {
1384  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\""
1385  << std::endl;
1386  switch (this->projModel) {
1388  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1389  break;
1391  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1392  break;
1395  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1396  break;
1397  }
1398  } else {
1399  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1400  "resolution");
1401  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1402  }
1403  break;
1404  }
1406  // Set default intrinsic camera parameters for 640x480 images
1407  if (image_width == 640 && image_height == 480) {
1408  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\""
1409  << std::endl;
1410  switch (this->projModel) {
1412  cam.initPersProjWithoutDistortion(605.4, 605.6, 328.6, 241.0);
1413  break;
1415  cam.initPersProjWithDistortion(611.8, 612.6, 327.8, 241.7, 0.0436, -0.04265);
1416  break;
1419  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1420  break;
1421  }
1422  } else {
1423  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1424  "resolution");
1425  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1426  }
1427  break;
1428  }
1430  // Set default intrinsic camera parameters for 640x480 images
1431  if (image_width == 640 && image_height == 480) {
1432  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\""
1433  << std::endl;
1434  switch (this->projModel) {
1436  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1437  break;
1439  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1440  break;
1443  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1444  break;
1445  }
1446  } else {
1447  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1448  "resolution");
1449  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1450  }
1451  break;
1452  }
1453  default:
1454  vpERROR_TRACE("This error should not occur!");
1455  break;
1456  }
1457 #endif
1458  return;
1459 }
1460 
1504 {
1505  getCameraParameters(cam, I.getWidth(), I.getHeight());
1506 }
1551 {
1552  getCameraParameters(cam, I.getWidth(), I.getHeight());
1553 }
1554 
1564 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6)
1565 {
1566  vpRotationMatrix eRc;
1567  afma6._eMc.extract(eRc);
1568  vpRxyzVector rxyz(eRc);
1569 
1570  os << "Joint Max:" << std::endl
1571  << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t"
1572  << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl
1573 
1574  << "Joint Min: " << std::endl
1575  << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t"
1576  << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl
1577 
1578  << "Long 5-6: " << std::endl
1579  << "\t" << afma6._long_56 << "\t" << std::endl
1580 
1581  << "Coupling 5-6:" << std::endl
1582  << "\t" << afma6._coupl_56 << "\t" << std::endl
1583 
1584  << "eMc: " << std::endl
1585  << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t"
1586  << std::endl
1587  << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1588  << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1589  << "\t" << std::endl;
1590 
1591  return os;
1592 }
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:79
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:94
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:102
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:87
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1197
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:86
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:920
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:92
vpColVector getJointMax() const
Definition: vpAfma6.cpp:1069
static const std::string CONST_CAMERA_AFMA6_FILENAME
Definition: vpAfma6.h:96
double _joint_min[6]
Definition: vpAfma6.h:204
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma6.cpp:530
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:194
void init(void)
Definition: vpAfma6.cpp:160
vpRxyzVector _erc
Definition: vpAfma6.h:207
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:610
static const char *const CONST_VACUUM_CAMERA_NAME
Definition: vpAfma6.h:112
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma6.cpp:845
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:88
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:898
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:194
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:169
double _coupl_56
Definition: vpAfma6.h:201
static const std::string CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:93
vpColVector getJointMin() const
Definition: vpAfma6.cpp:1053
static const char *const CONST_INTEL_D435_CAMERA_NAME
Definition: vpAfma6.h:123
double getCoupl56() const
Definition: vpAfma6.cpp:1083
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:136
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:107
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:89
vpAfma6()
Definition: vpAfma6.cpp:121
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:209
double _long_56
Definition: vpAfma6.h:202
void parseConfigFile(const std::string &filename)
Definition: vpAfma6.cpp:1103
vpTranslationVector _etc
Definition: vpAfma6.h:206
vpHomogeneousMatrix get_eMc() const
Definition: vpAfma6.cpp:909
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1266
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:785
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpAfma6.h:117
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:215
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:90
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:95
double _joint_max[6]
Definition: vpAfma6.h:203
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:942
static const std::string CONST_AFMA6_FILENAME
Definition: vpAfma6.h:85
double getLong56() const
Definition: vpAfma6.cpp:1091
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1012
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:126
@ TOOL_CCMOP
Definition: vpAfma6.h:127
@ TOOL_GENERIC_CAMERA
Definition: vpAfma6.h:130
@ TOOL_CUSTOM
Definition: vpAfma6.h:132
@ TOOL_VACUUM
Definition: vpAfma6.h:129
@ TOOL_INTEL_D435_CAMERA
Definition: vpAfma6.h:131
@ TOOL_GRIPPER
Definition: vpAfma6.h:128
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:494
unsigned int getRows() const
Definition: vpArray2D.h:291
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ notImplementedError
Not implemented.
Definition: vpException.h:93
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static double rad(double deg)
Definition: vpMath.h:117
static double deg(double rad)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Error that can be emited by the vpRobot class and its derivates.
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
XML parser to load and save intrinsic camera parameters.
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)
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393