Visual Servoing Platform  version 3.6.1 under development (2023-12-07)
vpAfma6.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
44 #include <iostream>
45 #include <sstream>
46 
47 #include <visp3/core/vpCameraParameters.h>
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpRotationMatrix.h>
50 #include <visp3/core/vpRxyzVector.h>
51 #include <visp3/core/vpTranslationVector.h>
52 #include <visp3/core/vpVelocityTwistMatrix.h>
53 #include <visp3/core/vpXmlParserCamera.h>
54 #include <visp3/robot/vpAfma6.h>
55 #include <visp3/robot/vpRobotException.h>
56 
57 /* ----------------------------------------------------------------------- */
58 /* --- STATIC ------------------------------------------------------------ */
59 /* ---------------------------------------------------------------------- */
60 
61 static const char *opt_Afma6[] = {"JOINT_MAX", "JOINT_MIN", "LONG_56", "COUPL_56",
62  "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr};
63 
64 #ifdef VISP_HAVE_AFMA6_DATA
65 const std::string vpAfma6::CONST_AFMA6_FILENAME =
66  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf");
67 
69  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf");
70 
72  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf");
73 
75  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf");
76 
78  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf");
79 
81  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf");
82 
84  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf");
85 
87  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf");
88 
90  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf");
91 
93  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf");
94 
96  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf");
97 
98 const std::string vpAfma6::CONST_CAMERA_AFMA6_FILENAME =
99  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml");
100 
101 #endif // VISP_HAVE_AFMA6_DATA
102 
103 const char *const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
104 const char *const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
105 const char *const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
106 const char *const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
107 const char *const vpAfma6::CONST_INTEL_D435_CAMERA_NAME = "Intel-D435";
108 
110 
111 const unsigned int vpAfma6::njoint = 6;
112 
119  : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(vpAfma6::defaultTool),
120  projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
121 {
122  // Set the default parameters in case of the config files are not available.
123 
124  //
125  // Geometric model constant parameters
126  //
127  // coupling between join 5 and 6
128  this->_coupl_56 = 0.009091;
129  // distance between join 5 and 6
130  this->_long_56 = -0.06924;
131  // Camera extrinsic parameters: effector to camera frame
132  this->_eMc.eye(); // Default values are initialized ...
133  // ... in init (vpAfma6::vpAfma6ToolType tool,
134  // vpCameraParameters::vpCameraParametersProjType projModel)
135  // Maximal value of the joints
136  this->_joint_max[0] = 0.7001;
137  this->_joint_max[1] = 0.5201;
138  this->_joint_max[2] = 0.4601;
139  this->_joint_max[3] = 2.7301;
140  this->_joint_max[4] = 2.4801;
141  this->_joint_max[5] = 1.5901;
142  // Minimal value of the joints
143  this->_joint_min[0] = -0.6501;
144  this->_joint_min[1] = -0.6001;
145  this->_joint_min[2] = -0.5001;
146  this->_joint_min[3] = -2.7301;
147  this->_joint_min[4] = -0.1001;
148  this->_joint_min[5] = -1.5901;
149 
150  init();
151 }
152 
157 void vpAfma6::init(void)
158 {
159  this->init(vpAfma6::defaultTool);
160  return;
161 }
162 
175 void vpAfma6::init(const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters)
176 {
177  this->parseConfigFile(camera_extrinsic_parameters);
178 
179  this->parseConfigFile(camera_intrinsic_parameters);
180 }
181 
212 void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
213 {
214  this->setToolType(tool);
215  this->parseConfigFile(filename.c_str());
216 }
217 
227 void vpAfma6::init(const std::string &camera_extrinsic_parameters)
228 {
229  this->parseConfigFile(camera_extrinsic_parameters);
230 }
231 
248 {
249  this->setToolType(tool);
250  this->set_eMc(eMc);
251 }
252 
271 {
272 
273  this->projModel = proj_model;
274 
275 #ifdef VISP_HAVE_AFMA6_DATA
276  // Read the robot parameters from files
277  std::string filename_eMc;
278  switch (tool) {
279  case vpAfma6::TOOL_CCMOP: {
280  switch (projModel) {
283  break;
286  break;
289  "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
290  break;
291  }
292  break;
293  }
294  case vpAfma6::TOOL_GRIPPER: {
295  switch (projModel) {
298  break;
301  break;
304  "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
305  break;
306  }
307  break;
308  }
309  case vpAfma6::TOOL_VACUUM: {
310  switch (projModel) {
313  break;
316  break;
319  "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;
334  "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
335  break;
336  }
337  break;
338  }
340  switch (projModel) {
343  break;
346  break;
349  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
350  break;
351  }
352  break;
353  }
354  default: {
355  vpERROR_TRACE("This error should not occur!");
356  break;
357  }
358  }
359 
360  this->init(vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
361 
362 #else // VISP_HAVE_AFMA6_DATA
363 
364  // Use here default values of the robot constant parameters.
365  switch (tool) {
366  case vpAfma6::TOOL_CCMOP: {
367  switch (projModel) {
369  _erc[0] = vpMath::rad(164.35); // rx
370  _erc[1] = vpMath::rad(89.64); // ry
371  _erc[2] = vpMath::rad(-73.05); // rz
372  _etc[0] = 0.0117; // tx
373  _etc[1] = 0.0033; // ty
374  _etc[2] = 0.2272; // tz
375  break;
377  _erc[0] = vpMath::rad(33.54); // rx
378  _erc[1] = vpMath::rad(89.34); // ry
379  _erc[2] = vpMath::rad(57.83); // rz
380  _etc[0] = 0.0373; // tx
381  _etc[1] = 0.0024; // ty
382  _etc[2] = 0.2286; // tz
383  break;
386  "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
387  break;
388  }
389  break;
390  }
391  case vpAfma6::TOOL_GRIPPER: {
392  switch (projModel) {
394  _erc[0] = vpMath::rad(88.33); // rx
395  _erc[1] = vpMath::rad(72.07); // ry
396  _erc[2] = vpMath::rad(2.53); // rz
397  _etc[0] = 0.0783; // tx
398  _etc[1] = 0.1234; // ty
399  _etc[2] = 0.1638; // tz
400  break;
402  _erc[0] = vpMath::rad(86.69); // rx
403  _erc[1] = vpMath::rad(71.93); // ry
404  _erc[2] = vpMath::rad(4.17); // rz
405  _etc[0] = 0.1034; // tx
406  _etc[1] = 0.1142; // ty
407  _etc[2] = 0.1642; // tz
408  break;
411  "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
412  break;
413  }
414  break;
415  }
416  case vpAfma6::TOOL_VACUUM: {
417  switch (projModel) {
419  _erc[0] = vpMath::rad(90.40); // rx
420  _erc[1] = vpMath::rad(75.11); // ry
421  _erc[2] = vpMath::rad(0.18); // rz
422  _etc[0] = 0.0038; // tx
423  _etc[1] = 0.1281; // ty
424  _etc[2] = 0.1658; // tz
425  break;
427  _erc[0] = vpMath::rad(91.61); // rx
428  _erc[1] = vpMath::rad(76.17); // ry
429  _erc[2] = vpMath::rad(-0.98); // rz
430  _etc[0] = 0.0815; // tx
431  _etc[1] = 0.1162; // ty
432  _etc[2] = 0.1658; // tz
433  break;
436  "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
437  break;
438  }
439  break;
440  }
442  switch (projModel) {
444  _erc[0] = vpMath::rad(-71.41); // rx
445  _erc[1] = vpMath::rad(89.49); // ry
446  _erc[2] = vpMath::rad(162.07); // rz
447  _etc[0] = 0.0038; // tx
448  _etc[1] = 0.1281; // ty
449  _etc[2] = 0.1658; // tz
450  break;
452  _erc[0] = vpMath::rad(-52.79); // rx
453  _erc[1] = vpMath::rad(89.55); // ry
454  _erc[2] = vpMath::rad(143.34); // rz
455  _etc[0] = 0.0693; // tx
456  _etc[1] = -0.0297; // ty
457  _etc[2] = 0.1357; // tz
458  break;
461  "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
462  break;
463  }
464  break;
465  }
468  switch (projModel) {
471  // set eMc to identity
472  _erc[0] = 0; // rx
473  _erc[1] = 0; // ry
474  _erc[2] = 0; // rz
475  _etc[0] = 0; // tx
476  _etc[1] = 0; // ty
477  _etc[2] = 0; // tz
478  break;
481  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
482  break;
483  }
484  break;
485  }
486  }
487  vpRotationMatrix eRc(_erc);
488  this->_eMc.buildFrom(_etc, eRc);
489 #endif // VISP_HAVE_AFMA6_DATA
490 
491  setToolType(tool);
492  return;
493 }
494 
520 {
522  fMc = get_fMc(q);
523 
524  return fMc;
525 }
526 
599 int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest,
600  const bool &verbose) const
601 {
603  double q_[2][6], d[2], t;
604  int ok[2];
605  double cord[6];
606 
607  int nbsol = 0;
608 
609  if (q.getRows() != njoint)
610  q.resize(6);
611 
612  // for(unsigned int i=0;i<3;i++) {
613  // fMe[i][3] = fMc[i][3];
614  // for(int j=0;j<3;j++) {
615  // fMe[i][j] = 0.0;
616  // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
617  // fMe[i][3] -= fMe[i][j]*rpi[j][3];
618  // }
619  // }
620 
621  // std::cout << "\n\nfMc: " << fMc;
622  // std::cout << "\n\neMc: " << _eMc;
623 
624  fMe = fMc * this->_eMc.inverse();
625  // std::cout << "\n\nfMe: " << fMe;
626 
627  if (fMe[2][2] >= .99999f) {
628  vpTRACE("singularity\n");
629  q_[0][4] = q_[1][4] = M_PI / 2.f;
630  t = atan2(fMe[0][0], fMe[0][1]);
631  q_[1][3] = q_[0][3] = q[3];
632  q_[1][5] = q_[0][5] = t - q_[0][3];
633 
634  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
635  /* -> a cause du couplage 4/5 */
636  {
637  q_[1][5] -= vpMath::rad(10);
638  q_[1][3] += vpMath::rad(10);
639  }
640  while (q_[1][5] <= this->_joint_min[5]) {
641  q_[1][5] += vpMath::rad(10);
642  q_[1][3] -= vpMath::rad(10);
643  }
644  } else if (fMe[2][2] <= -.99999) {
645  vpTRACE("singularity\n");
646  q_[0][4] = q_[1][4] = -M_PI / 2;
647  t = atan2(fMe[1][1], fMe[1][0]);
648  q_[1][3] = q_[0][3] = q[3];
649  q_[1][5] = q_[0][5] = q_[0][3] - t;
650  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
651  /* -> a cause du couplage 4/5 */
652  {
653  q_[1][5] -= vpMath::rad(10);
654  q_[1][3] -= vpMath::rad(10);
655  }
656  while (q_[1][5] <= this->_joint_min[5]) {
657  q_[1][5] += vpMath::rad(10);
658  q_[1][3] += vpMath::rad(10);
659  }
660  } else {
661  q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
662  if (q_[0][3] >= 0.0)
663  q_[1][3] = q_[0][3] - M_PI;
664  else
665  q_[1][3] = q_[0][3] + M_PI;
666 
667  q_[0][4] = asin(fMe[2][2]);
668  if (q_[0][4] >= 0.0)
669  q_[1][4] = M_PI - q_[0][4];
670  else
671  q_[1][4] = -M_PI - q_[0][4];
672 
673  q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
674  if (q_[0][5] >= 0.0)
675  q_[1][5] = q_[0][5] - M_PI;
676  else
677  q_[1][5] = q_[0][5] + M_PI;
678  }
679  q_[0][0] = fMe[0][3] - this->_long_56 * cos(q_[0][3]);
680  q_[1][0] = fMe[0][3] - this->_long_56 * cos(q_[1][3]);
681  q_[0][1] = fMe[1][3] - this->_long_56 * sin(q_[0][3]);
682  q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]);
683  q_[0][2] = q_[1][2] = fMe[2][3];
684 
685  /* prise en compte du couplage axes 5/6 */
686  q_[0][5] += this->_coupl_56 * q_[0][4];
687  q_[1][5] += this->_coupl_56 * q_[1][4];
688 
689  for (int j = 0; j < 2; j++) {
690  ok[j] = 1;
691  // test is position is reachable
692  for (unsigned int i = 0; i < 6; i++) {
693  if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
694  if (verbose) {
695  if (i < 3)
696  std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < "
697  << this->_joint_max[i] << std::endl;
698  else
699  std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < "
700  << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
701  }
702  ok[j] = 0;
703  }
704  }
705  }
706  if (ok[0] == 0) {
707  if (ok[1] == 0) {
708  std::cout << "No solution..." << std::endl;
709  nbsol = 0;
710  return nbsol;
711  } else if (ok[1] == 1) {
712  for (unsigned int i = 0; i < 6; i++)
713  cord[i] = q_[1][i];
714  nbsol = 1;
715  }
716  } else {
717  if (ok[1] == 0) {
718  for (unsigned int i = 0; i < 6; i++)
719  cord[i] = q_[0][i];
720  nbsol = 1;
721  } else {
722  nbsol = 2;
723  // vpTRACE("2 solutions\n");
724  for (int j = 0; j < 2; j++) {
725  d[j] = 0.0;
726  for (unsigned int i = 3; i < 6; i++)
727  d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
728  }
729  if (nearest == true) {
730  if (d[0] <= d[1])
731  for (unsigned int i = 0; i < 6; i++)
732  cord[i] = q_[0][i];
733  else
734  for (unsigned int i = 0; i < 6; i++)
735  cord[i] = q_[1][i];
736  } else {
737  if (d[0] <= d[1])
738  for (unsigned int i = 0; i < 6; i++)
739  cord[i] = q_[1][i];
740  else
741  for (unsigned int i = 0; i < 6; i++)
742  cord[i] = q_[0][i];
743  }
744  }
745  }
746  for (unsigned int i = 0; i < 6; i++)
747  q[i] = cord[i];
748 
749  return nbsol;
750 }
751 
775 {
777  get_fMc(q, fMc);
778 
779  return fMc;
780 }
781 
802 {
803 
804  // Compute the direct geometric model: fMe = transformation between
805  // fix and end effector frame.
807 
808  get_fMe(q, fMe);
809 
810  fMc = fMe * this->_eMc;
811 
812  return;
813 }
814 
835 {
836  double q0 = q[0]; // meter
837  double q1 = q[1]; // meter
838  double q2 = q[2]; // meter
839 
840  /* Decouplage liaisons 2 et 3. */
841  double q5 = q[5] - this->_coupl_56 * q[4];
842 
843  double c1 = cos(q[3]);
844  double s1 = sin(q[3]);
845  double c2 = cos(q[4]);
846  double s2 = sin(q[4]);
847  double c3 = cos(q5);
848  double s3 = sin(q5);
849 
850  // Compute the direct geometric model: fMe = transformation betwee
851  // fix and end effector frame.
852  fMe[0][0] = s1 * s2 * c3 + c1 * s3;
853  fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
854  fMe[0][2] = -s1 * c2;
855  fMe[0][3] = q0 + this->_long_56 * c1;
856 
857  fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
858  fMe[1][1] = c1 * s2 * s3 + s1 * c3;
859  fMe[1][2] = c1 * c2;
860  fMe[1][3] = q1 + this->_long_56 * s1;
861 
862  fMe[2][0] = c2 * c3;
863  fMe[2][1] = -c2 * s3;
864  fMe[2][2] = s2;
865  fMe[2][3] = q2;
866 
867  fMe[3][0] = 0;
868  fMe[3][1] = 0;
869  fMe[3][2] = 0;
870  fMe[3][3] = 1;
871 
872  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
873 
874  return;
875 }
876 
887 void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
898 vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
899 
910 {
912  get_cMe(cMe);
913 
914  cVe.buildFrom(cMe);
915 
916  return;
917 }
918 
931 void vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
932 {
933 
934  eJe.resize(6, 6);
935 
936  double s4, c4, s5, c5, s6, c6;
937 
938  s4 = sin(q[3]);
939  c4 = cos(q[3]);
940  s5 = sin(q[4]);
941  c5 = cos(q[4]);
942  s6 = sin(q[5] - this->_coupl_56 * q[4]);
943  c6 = cos(q[5] - this->_coupl_56 * q[4]);
944 
945  eJe = 0;
946  eJe[0][0] = s4 * s5 * c6 + c4 * s6;
947  eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
948  eJe[0][2] = c5 * c6;
949  eJe[0][3] = -this->_long_56 * s5 * c6;
950 
951  eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
952  eJe[1][1] = c4 * s5 * s6 + s4 * c6;
953  eJe[1][2] = -c5 * s6;
954  eJe[1][3] = this->_long_56 * s5 * s6;
955 
956  eJe[2][0] = -s4 * c5;
957  eJe[2][1] = c4 * c5;
958  eJe[2][2] = s5;
959  eJe[2][3] = this->_long_56 * c5;
960 
961  eJe[3][3] = c5 * c6;
962  eJe[3][4] = s6;
963 
964  eJe[4][3] = -c5 * s6;
965  eJe[4][4] = c6;
966 
967  eJe[5][3] = s5;
968  eJe[5][4] = -this->_coupl_56;
969  eJe[5][5] = 1;
970 
971  return;
972 }
973 
1001 void vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1002 {
1003 
1004  fJe.resize(6, 6);
1005 
1006  // block superieur gauche
1007  fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1008 
1009  double s4 = sin(q[3]);
1010  double c4 = cos(q[3]);
1011 
1012  // block superieur droit
1013  fJe[0][3] = -this->_long_56 * s4;
1014  fJe[1][3] = this->_long_56 * c4;
1015 
1016  double s5 = sin(q[4]);
1017  double c5 = cos(q[4]);
1018  // block inferieur droit
1019  fJe[3][4] = c4;
1020  fJe[3][5] = -s4 * c5;
1021  fJe[4][4] = s4;
1022  fJe[4][5] = c4 * c5;
1023  fJe[5][3] = 1;
1024  fJe[5][5] = s5;
1025 
1026  // coupling between joint 5 and 6
1027  fJe[3][4] += this->_coupl_56 * s4 * c5;
1028  fJe[4][4] += -this->_coupl_56 * c4 * c5;
1029  fJe[5][4] += -this->_coupl_56 * s5;
1030 
1031  return;
1032 }
1033 
1043 {
1044  vpColVector qmin(6);
1045  for (unsigned int i = 0; i < 6; i++)
1046  qmin[i] = this->_joint_min[i];
1047  return qmin;
1048 }
1049 
1059 {
1060  vpColVector qmax(6);
1061  for (unsigned int i = 0; i < 6; i++)
1062  qmax[i] = this->_joint_max[i];
1063  return qmax;
1064 }
1065 
1072 double vpAfma6::getCoupl56() const { return _coupl_56; }
1073 
1080 double vpAfma6::getLong56() const { return _long_56; }
1081 
1092 void vpAfma6::parseConfigFile(const std::string &filename)
1093 {
1094  vpRxyzVector erc; // eMc rotation
1095  vpTranslationVector etc; // eMc translation
1096 
1097  std::ifstream fdconfig(filename.c_str(), std::ios::in);
1098 
1099  if (!fdconfig.is_open()) {
1100  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
1101  filename.c_str());
1102  }
1103 
1104  std::string line;
1105  int lineNum = 0;
1106  bool get_erc = false;
1107  bool get_etc = false;
1108  int code;
1109 
1110  while (std::getline(fdconfig, line)) {
1111  lineNum++;
1112  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
1113  continue;
1114  }
1115  std::istringstream ss(line);
1116  std::string key;
1117  ss >> key;
1118 
1119  for (code = 0; nullptr != opt_Afma6[code]; ++code) {
1120  if (key.compare(opt_Afma6[code]) == 0) {
1121  break;
1122  }
1123  }
1124 
1125  switch (code) {
1126  case 0:
1127  ss >> this->_joint_max[0] >> this->_joint_max[1] >> this->_joint_max[2] >> this->_joint_max[3] >>
1128  this->_joint_max[4] >> this->_joint_max[5];
1129  break;
1130 
1131  case 1:
1132  ss >> this->_joint_min[0] >> this->_joint_min[1] >> this->_joint_min[2] >> this->_joint_min[3] >>
1133  this->_joint_min[4] >> this->_joint_min[5];
1134  break;
1135 
1136  case 2:
1137  ss >> this->_long_56;
1138  break;
1139 
1140  case 3:
1141  ss >> this->_coupl_56;
1142  break;
1143 
1144  case 4:
1145  break; // Nothing to do: camera name
1146 
1147  case 5:
1148  ss >> erc[0] >> erc[1] >> erc[2];
1149 
1150  // Convert rotation from degrees to radians
1151  erc = erc * M_PI / 180.0;
1152  get_erc = true;
1153  break;
1154 
1155  case 6:
1156  ss >> etc[0] >> etc[1] >> etc[2];
1157  get_etc = true;
1158  break;
1159 
1160  default:
1161  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
1162  filename.c_str(), lineNum));
1163  }
1164  }
1165 
1166  fdconfig.close();
1167 
1168  // Compute the eMc matrix from the translations and rotations
1169  if (get_etc && get_erc) {
1170  _erc = erc;
1171  _etc = etc;
1172 
1173  vpRotationMatrix eRc(_erc);
1174  this->_eMc.buildFrom(_etc, eRc);
1175  }
1176 }
1177 
1187 {
1188  this->_eMc = eMc;
1189  this->_etc = eMc.getTranslationVector();
1190 
1191  vpRotationMatrix R(eMc);
1192  this->_erc.buildFrom(R);
1193 }
1194 
1255 void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
1256  const unsigned int &image_height) const
1257 {
1258 #if defined(VISP_HAVE_AFMA6_DATA)
1259  vpXmlParserCamera parser;
1260  switch (getToolType()) {
1261  case vpAfma6::TOOL_CCMOP: {
1262  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1263  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1265  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1266  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1267  }
1268  break;
1269  }
1270  case vpAfma6::TOOL_GRIPPER: {
1271  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1272  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1274  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1275  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1276  }
1277  break;
1278  }
1279  case vpAfma6::TOOL_VACUUM: {
1280  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1281  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1283  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1284  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1285  }
1286  break;
1287  }
1289  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\"" << std::endl
1290  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1292  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1293  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1294  }
1295  break;
1296  }
1298  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1299  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1301  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1302  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1303  }
1304  break;
1305  }
1306  default: {
1307  vpERROR_TRACE("This error should not occur!");
1308  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1309  // "que les specs de la classe ont ete modifiee, "
1310  // "et que le code n'a pas ete mis a jour "
1311  // "correctement.");
1312  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1313  // "vpAfma6::vpAfma6ToolType, et controlez que "
1314  // "tous les cas ont ete pris en compte dans la "
1315  // "fonction init(camera).");
1316  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1317  }
1318  }
1319 #else
1320  // Set default parameters
1321  switch (getToolType()) {
1322  case vpAfma6::TOOL_CCMOP: {
1323  // Set default intrinsic camera parameters for 640x480 images
1324  if (image_width == 640 && image_height == 480) {
1325  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
1326  << std::endl;
1327  switch (this->projModel) {
1329  cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1330  break;
1332  cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1333  break;
1336  "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;
1360  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1361  break;
1362  }
1363  } else {
1364  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1365  "resolution");
1366  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1367  }
1368  break;
1369  }
1370  case vpAfma6::TOOL_VACUUM: {
1371  // Set default intrinsic camera parameters for 640x480 images
1372  if (image_width == 640 && image_height == 480) {
1373  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\""
1374  << std::endl;
1375  switch (this->projModel) {
1377  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1378  break;
1380  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1381  break;
1384  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1385  break;
1386  }
1387  } else {
1388  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1389  "resolution");
1390  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1391  }
1392  break;
1393  }
1395  // Set default intrinsic camera parameters for 640x480 images
1396  if (image_width == 640 && image_height == 480) {
1397  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\""
1398  << std::endl;
1399  switch (this->projModel) {
1401  cam.initPersProjWithoutDistortion(605.4, 605.6, 328.6, 241.0);
1402  break;
1404  cam.initPersProjWithDistortion(611.8, 612.6, 327.8, 241.7, 0.0436, -0.04265);
1405  break;
1408  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1409  break;
1410  }
1411  } else {
1412  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1413  "resolution");
1414  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1415  }
1416  break;
1417  }
1419  // Set default intrinsic camera parameters for 640x480 images
1420  if (image_width == 640 && image_height == 480) {
1421  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\""
1422  << std::endl;
1423  switch (this->projModel) {
1425  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1426  break;
1428  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1429  break;
1432  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1433  break;
1434  }
1435  } else {
1436  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1437  "resolution");
1438  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1439  }
1440  break;
1441  }
1442  default:
1443  vpERROR_TRACE("This error should not occur!");
1444  break;
1445  }
1446 #endif
1447  return;
1448 }
1449 
1493 {
1494  getCameraParameters(cam, I.getWidth(), I.getHeight());
1495 }
1540 {
1541  getCameraParameters(cam, I.getWidth(), I.getHeight());
1542 }
1543 
1553 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6)
1554 {
1555  vpRotationMatrix eRc;
1556  afma6._eMc.extract(eRc);
1557  vpRxyzVector rxyz(eRc);
1558 
1559  os << "Joint Max:" << std::endl
1560  << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t"
1561  << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl
1562 
1563  << "Joint Min: " << std::endl
1564  << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t"
1565  << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl
1566 
1567  << "Long 5-6: " << std::endl
1568  << "\t" << afma6._long_56 << "\t" << std::endl
1569 
1570  << "Coupling 5-6:" << std::endl
1571  << "\t" << afma6._coupl_56 << "\t" << std::endl
1572 
1573  << "eMc: " << std::endl
1574  << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t"
1575  << std::endl
1576  << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1577  << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1578  << "\t" << std::endl;
1579 
1580  return os;
1581 }
Modelization of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:76
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:91
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:99
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:84
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1186
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:83
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:909
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:89
vpColVector getJointMax() const
Definition: vpAfma6.cpp:1058
static const std::string CONST_CAMERA_AFMA6_FILENAME
Definition: vpAfma6.h:93
double _joint_min[6]
Definition: vpAfma6.h:201
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma6.cpp:519
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:191
void init(void)
Definition: vpAfma6.cpp:157
vpRxyzVector _erc
Definition: vpAfma6.h:204
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:599
static const char *const CONST_VACUUM_CAMERA_NAME
Definition: vpAfma6.h:109
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma6.cpp:834
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:85
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:887
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:191
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:166
double _coupl_56
Definition: vpAfma6.h:198
static const std::string CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:90
vpColVector getJointMin() const
Definition: vpAfma6.cpp:1042
static const char *const CONST_INTEL_D435_CAMERA_NAME
Definition: vpAfma6.h:120
double getCoupl56() const
Definition: vpAfma6.cpp:1072
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:133
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:104
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:86
vpAfma6()
Definition: vpAfma6.cpp:118
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:206
double _long_56
Definition: vpAfma6.h:199
void parseConfigFile(const std::string &filename)
Definition: vpAfma6.cpp:1092
vpTranslationVector _etc
Definition: vpAfma6.h:203
vpHomogeneousMatrix get_eMc() const
Definition: vpAfma6.cpp:898
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1255
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:774
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpAfma6.h:114
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:212
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:87
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:92
double _joint_max[6]
Definition: vpAfma6.h:200
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:88
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:931
static const std::string CONST_AFMA6_FILENAME
Definition: vpAfma6.h:82
double getLong56() const
Definition: vpAfma6.cpp:1080
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1001
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:123
@ TOOL_CCMOP
Definition: vpAfma6.h:124
@ TOOL_GENERIC_CAMERA
Definition: vpAfma6.h:127
@ TOOL_CUSTOM
Definition: vpAfma6.h:129
@ TOOL_VACUUM
Definition: vpAfma6.h:126
@ TOOL_INTEL_D435_CAMERA
Definition: vpAfma6.h:128
@ TOOL_GRIPPER
Definition: vpAfma6.h:125
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:282
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:504
unsigned int getRows() const
Definition: vpArray2D.h:267
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
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:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1049
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ notImplementedError
Not implemented.
Definition: vpException.h:81
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:240
unsigned int getHeight() const
Definition: vpImage.h:182
static double rad(double deg)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Error that can be emitted by the vpRobot class and its derivatives.
@ readingParametersError
Cannot parse parameters.
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:176
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, bool verbose=true)
#define vpTRACE
Definition: vpDebug.h:411
#define vpERROR_TRACE
Definition: vpDebug.h:388