Visual Servoing Platform  version 3.4.0
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 }
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:1187
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:86
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:910
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:92
vpColVector getJointMax() const
Definition: vpAfma6.cpp:1059
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:520
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:600
static const char *const CONST_VACUUM_CAMERA_NAME
Definition: vpAfma6.h:112
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma6.cpp:835
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:88
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:888
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:1043
static const char *const CONST_INTEL_D435_CAMERA_NAME
Definition: vpAfma6.h:123
double getCoupl56() const
Definition: vpAfma6.cpp:1073
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:1093
vpTranslationVector _etc
Definition: vpAfma6.h:206
vpHomogeneousMatrix get_eMc() const
Definition: vpAfma6.cpp:899
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1256
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:775
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:932
static const std::string CONST_AFMA6_FILENAME
Definition: vpAfma6.h:85
double getLong56() const
Definition: vpAfma6.cpp:1081
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1002
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:304
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
unsigned int getRows() const
Definition: vpArray2D.h:289
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:310
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:110
static double deg(double rad)
Definition: vpMath.h:103
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