Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
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.build(_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  }
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  }
662  else {
663  q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
664  if (q_[0][3] >= 0.0)
665  q_[1][3] = q_[0][3] - M_PI;
666  else
667  q_[1][3] = q_[0][3] + M_PI;
668 
669  q_[0][4] = asin(fMe[2][2]);
670  if (q_[0][4] >= 0.0)
671  q_[1][4] = M_PI - q_[0][4];
672  else
673  q_[1][4] = -M_PI - q_[0][4];
674 
675  q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
676  if (q_[0][5] >= 0.0)
677  q_[1][5] = q_[0][5] - M_PI;
678  else
679  q_[1][5] = q_[0][5] + M_PI;
680  }
681  q_[0][0] = fMe[0][3] - this->_long_56 * cos(q_[0][3]);
682  q_[1][0] = fMe[0][3] - this->_long_56 * cos(q_[1][3]);
683  q_[0][1] = fMe[1][3] - this->_long_56 * sin(q_[0][3]);
684  q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]);
685  q_[0][2] = q_[1][2] = fMe[2][3];
686 
687  /* prise en compte du couplage axes 5/6 */
688  q_[0][5] += this->_coupl_56 * q_[0][4];
689  q_[1][5] += this->_coupl_56 * q_[1][4];
690 
691  for (int j = 0; j < 2; j++) {
692  ok[j] = 1;
693  // test is position is reachable
694  for (unsigned int i = 0; i < 6; i++) {
695  if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
696  if (verbose) {
697  if (i < 3)
698  std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < "
699  << this->_joint_max[i] << std::endl;
700  else
701  std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < "
702  << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
703  }
704  ok[j] = 0;
705  }
706  }
707  }
708  if (ok[0] == 0) {
709  if (ok[1] == 0) {
710  std::cout << "No solution..." << std::endl;
711  nbsol = 0;
712  return nbsol;
713  }
714  else if (ok[1] == 1) {
715  for (unsigned int i = 0; i < 6; i++)
716  cord[i] = q_[1][i];
717  nbsol = 1;
718  }
719  }
720  else {
721  if (ok[1] == 0) {
722  for (unsigned int i = 0; i < 6; i++)
723  cord[i] = q_[0][i];
724  nbsol = 1;
725  }
726  else {
727  nbsol = 2;
728  // vpTRACE("2 solutions\n");
729  for (int j = 0; j < 2; j++) {
730  d[j] = 0.0;
731  for (unsigned int i = 3; i < 6; i++)
732  d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
733  }
734  if (nearest == true) {
735  if (d[0] <= d[1])
736  for (unsigned int i = 0; i < 6; i++)
737  cord[i] = q_[0][i];
738  else
739  for (unsigned int i = 0; i < 6; i++)
740  cord[i] = q_[1][i];
741  }
742  else {
743  if (d[0] <= d[1])
744  for (unsigned int i = 0; i < 6; i++)
745  cord[i] = q_[1][i];
746  else
747  for (unsigned int i = 0; i < 6; i++)
748  cord[i] = q_[0][i];
749  }
750  }
751  }
752  for (unsigned int i = 0; i < 6; i++)
753  q[i] = cord[i];
754 
755  return nbsol;
756 }
757 
781 {
783  get_fMc(q, fMc);
784 
785  return fMc;
786 }
787 
808 {
809 
810  // Compute the direct geometric model: fMe = transformation between
811  // fix and end effector frame.
813 
814  get_fMe(q, fMe);
815 
816  fMc = fMe * this->_eMc;
817 
818  return;
819 }
820 
841 {
842  double q0 = q[0]; // meter
843  double q1 = q[1]; // meter
844  double q2 = q[2]; // meter
845 
846  /* Decouplage liaisons 2 et 3. */
847  double q5 = q[5] - this->_coupl_56 * q[4];
848 
849  double c1 = cos(q[3]);
850  double s1 = sin(q[3]);
851  double c2 = cos(q[4]);
852  double s2 = sin(q[4]);
853  double c3 = cos(q5);
854  double s3 = sin(q5);
855 
856  // Compute the direct geometric model: fMe = transformation betwee
857  // fix and end effector frame.
858  fMe[0][0] = s1 * s2 * c3 + c1 * s3;
859  fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
860  fMe[0][2] = -s1 * c2;
861  fMe[0][3] = q0 + this->_long_56 * c1;
862 
863  fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
864  fMe[1][1] = c1 * s2 * s3 + s1 * c3;
865  fMe[1][2] = c1 * c2;
866  fMe[1][3] = q1 + this->_long_56 * s1;
867 
868  fMe[2][0] = c2 * c3;
869  fMe[2][1] = -c2 * s3;
870  fMe[2][2] = s2;
871  fMe[2][3] = q2;
872 
873  fMe[3][0] = 0;
874  fMe[3][1] = 0;
875  fMe[3][2] = 0;
876  fMe[3][3] = 1;
877 
878  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
879 
880  return;
881 }
882 
893 void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
904 vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
905 
916 {
918  get_cMe(cMe);
919 
920  cVe.build(cMe);
921 
922  return;
923 }
924 
937 void vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
938 {
939 
940  eJe.resize(6, 6);
941 
942  double s4, c4, s5, c5, s6, c6;
943 
944  s4 = sin(q[3]);
945  c4 = cos(q[3]);
946  s5 = sin(q[4]);
947  c5 = cos(q[4]);
948  s6 = sin(q[5] - this->_coupl_56 * q[4]);
949  c6 = cos(q[5] - this->_coupl_56 * q[4]);
950 
951  eJe = 0;
952  eJe[0][0] = s4 * s5 * c6 + c4 * s6;
953  eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
954  eJe[0][2] = c5 * c6;
955  eJe[0][3] = -this->_long_56 * s5 * c6;
956 
957  eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
958  eJe[1][1] = c4 * s5 * s6 + s4 * c6;
959  eJe[1][2] = -c5 * s6;
960  eJe[1][3] = this->_long_56 * s5 * s6;
961 
962  eJe[2][0] = -s4 * c5;
963  eJe[2][1] = c4 * c5;
964  eJe[2][2] = s5;
965  eJe[2][3] = this->_long_56 * c5;
966 
967  eJe[3][3] = c5 * c6;
968  eJe[3][4] = s6;
969 
970  eJe[4][3] = -c5 * s6;
971  eJe[4][4] = c6;
972 
973  eJe[5][3] = s5;
974  eJe[5][4] = -this->_coupl_56;
975  eJe[5][5] = 1;
976 
977  return;
978 }
979 
1007 void vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1008 {
1009 
1010  fJe.resize(6, 6);
1011 
1012  // block superieur gauche
1013  fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1014 
1015  double s4 = sin(q[3]);
1016  double c4 = cos(q[3]);
1017 
1018  // block superieur droit
1019  fJe[0][3] = -this->_long_56 * s4;
1020  fJe[1][3] = this->_long_56 * c4;
1021 
1022  double s5 = sin(q[4]);
1023  double c5 = cos(q[4]);
1024  // block inferieur droit
1025  fJe[3][4] = c4;
1026  fJe[3][5] = -s4 * c5;
1027  fJe[4][4] = s4;
1028  fJe[4][5] = c4 * c5;
1029  fJe[5][3] = 1;
1030  fJe[5][5] = s5;
1031 
1032  // coupling between joint 5 and 6
1033  fJe[3][4] += this->_coupl_56 * s4 * c5;
1034  fJe[4][4] += -this->_coupl_56 * c4 * c5;
1035  fJe[5][4] += -this->_coupl_56 * s5;
1036 
1037  return;
1038 }
1039 
1049 {
1050  vpColVector qmin(6);
1051  for (unsigned int i = 0; i < 6; i++)
1052  qmin[i] = this->_joint_min[i];
1053  return qmin;
1054 }
1055 
1065 {
1066  vpColVector qmax(6);
1067  for (unsigned int i = 0; i < 6; i++)
1068  qmax[i] = this->_joint_max[i];
1069  return qmax;
1070 }
1071 
1078 double vpAfma6::getCoupl56() const { return _coupl_56; }
1079 
1086 double vpAfma6::getLong56() const { return _long_56; }
1087 
1098 void vpAfma6::parseConfigFile(const std::string &filename)
1099 {
1100  vpRxyzVector erc; // eMc rotation
1101  vpTranslationVector etc; // eMc translation
1102 
1103  std::ifstream fdconfig(filename.c_str(), std::ios::in);
1104 
1105  if (!fdconfig.is_open()) {
1106  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
1107  filename.c_str());
1108  }
1109 
1110  std::string line;
1111  int lineNum = 0;
1112  bool get_erc = false;
1113  bool get_etc = false;
1114  int code;
1115 
1116  while (std::getline(fdconfig, line)) {
1117  lineNum++;
1118  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
1119  continue;
1120  }
1121  std::istringstream ss(line);
1122  std::string key;
1123  ss >> key;
1124 
1125  for (code = 0; nullptr != opt_Afma6[code]; ++code) {
1126  if (key.compare(opt_Afma6[code]) == 0) {
1127  break;
1128  }
1129  }
1130 
1131  switch (code) {
1132  case 0:
1133  ss >> this->_joint_max[0] >> this->_joint_max[1] >> this->_joint_max[2] >> this->_joint_max[3] >>
1134  this->_joint_max[4] >> this->_joint_max[5];
1135  break;
1136 
1137  case 1:
1138  ss >> this->_joint_min[0] >> this->_joint_min[1] >> this->_joint_min[2] >> this->_joint_min[3] >>
1139  this->_joint_min[4] >> this->_joint_min[5];
1140  break;
1141 
1142  case 2:
1143  ss >> this->_long_56;
1144  break;
1145 
1146  case 3:
1147  ss >> this->_coupl_56;
1148  break;
1149 
1150  case 4:
1151  break; // Nothing to do: camera name
1152 
1153  case 5:
1154  ss >> erc[0] >> erc[1] >> erc[2];
1155 
1156  // Convert rotation from degrees to radians
1157  erc = erc * M_PI / 180.0;
1158  get_erc = true;
1159  break;
1160 
1161  case 6:
1162  ss >> etc[0] >> etc[1] >> etc[2];
1163  get_etc = true;
1164  break;
1165 
1166  default:
1167  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
1168  filename.c_str(), lineNum));
1169  }
1170  }
1171 
1172  fdconfig.close();
1173 
1174  // Compute the eMc matrix from the translations and rotations
1175  if (get_etc && get_erc) {
1176  _erc = erc;
1177  _etc = etc;
1178 
1179  vpRotationMatrix eRc(_erc);
1180  this->_eMc.build(_etc, eRc);
1181  }
1182 }
1183 
1193 {
1194  this->_eMc = eMc;
1195  this->_etc = eMc.getTranslationVector();
1196 
1197  vpRotationMatrix R(eMc);
1198  this->_erc.build(R);
1199 }
1200 
1261 void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
1262  const unsigned int &image_height) const
1263 {
1264 #if defined(VISP_HAVE_AFMA6_DATA) && defined(VISP_HAVE_PUGIXML)
1265  vpXmlParserCamera parser;
1266  switch (getToolType()) {
1267  case vpAfma6::TOOL_CCMOP: {
1268  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1269  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1271  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1272  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1273  }
1274  break;
1275  }
1276  case vpAfma6::TOOL_GRIPPER: {
1277  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1278  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1280  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1281  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1282  }
1283  break;
1284  }
1285  case vpAfma6::TOOL_VACUUM: {
1286  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1287  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1289  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1290  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1291  }
1292  break;
1293  }
1295  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\"" << std::endl
1296  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1298  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1299  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1300  }
1301  break;
1302  }
1304  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1305  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1307  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1308  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1309  }
1310  break;
1311  }
1312  default: {
1313  vpERROR_TRACE("This error should not occur!");
1314  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1315  // "que les specs de la classe ont ete modifiee, "
1316  // "et que le code n'a pas ete mis a jour "
1317  // "correctement.");
1318  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1319  // "vpAfma6::vpAfma6ToolType, et controlez que "
1320  // "tous les cas ont ete pris en compte dans la "
1321  // "fonction init(camera).");
1322  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1323  }
1324  }
1325 #else
1326  // Set default parameters
1327  switch (getToolType()) {
1328  case vpAfma6::TOOL_CCMOP: {
1329  // Set default intrinsic camera parameters for 640x480 images
1330  if (image_width == 640 && image_height == 480) {
1331  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
1332  << std::endl;
1333  switch (this->projModel) {
1335  cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1336  break;
1338  cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1339  break;
1342  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1343  break;
1344  }
1345  }
1346  else {
1347  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1348  "resolution");
1349  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1350  }
1351  break;
1352  }
1353  case vpAfma6::TOOL_GRIPPER: {
1354  // Set default intrinsic camera parameters for 640x480 images
1355  if (image_width == 640 && image_height == 480) {
1356  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
1357  << std::endl;
1358  switch (this->projModel) {
1360  cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
1361  break;
1363  cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
1364  break;
1367  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1368  break;
1369  }
1370  }
1371  else {
1372  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1373  "resolution");
1374  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1375  }
1376  break;
1377  }
1378  case vpAfma6::TOOL_VACUUM: {
1379  // Set default intrinsic camera parameters for 640x480 images
1380  if (image_width == 640 && image_height == 480) {
1381  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\""
1382  << std::endl;
1383  switch (this->projModel) {
1385  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1386  break;
1388  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1389  break;
1392  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1393  break;
1394  }
1395  }
1396  else {
1397  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1398  "resolution");
1399  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1400  }
1401  break;
1402  }
1404  // Set default intrinsic camera parameters for 640x480 images
1405  if (image_width == 640 && image_height == 480) {
1406  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\""
1407  << std::endl;
1408  switch (this->projModel) {
1410  cam.initPersProjWithoutDistortion(605.4, 605.6, 328.6, 241.0);
1411  break;
1413  cam.initPersProjWithDistortion(611.8, 612.6, 327.8, 241.7, 0.0436, -0.04265);
1414  break;
1417  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1418  break;
1419  }
1420  }
1421  else {
1422  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1423  "resolution");
1424  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1425  }
1426  break;
1427  }
1429  // Set default intrinsic camera parameters for 640x480 images
1430  if (image_width == 640 && image_height == 480) {
1431  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\""
1432  << std::endl;
1433  switch (this->projModel) {
1435  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1436  break;
1438  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1439  break;
1442  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1443  break;
1444  }
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 }
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:1192
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:83
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:915
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:89
vpColVector getJointMax() const
Definition: vpAfma6.cpp:1064
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:840
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:85
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:893
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:1048
static const char *const CONST_INTEL_D435_CAMERA_NAME
Definition: vpAfma6.h:120
double getCoupl56() const
Definition: vpAfma6.cpp:1078
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:1098
vpTranslationVector _etc
Definition: vpAfma6.h:203
vpHomogeneousMatrix get_eMc() const
Definition: vpAfma6.cpp:904
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1261
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:780
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:937
static const std::string CONST_AFMA6_FILENAME
Definition: vpAfma6.h:82
double getLong56() const
Definition: vpAfma6.cpp:1086
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1007
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:354
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:603
unsigned int getRows() const
Definition: vpArray2D.h:339
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:1058
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ notImplementedError
Not implemented.
Definition: vpException.h:68
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
void extract(vpRotationMatrix &R) const
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
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:177
vpRxyzVector & build(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix & build(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:407
#define vpERROR_TRACE
Definition: vpDebug.h:384