Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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 
95 const std::string vpAfma6::CONST_CAMERA_AFMA6_FILENAME =
96  std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml");
97 
98 #endif // VISP_HAVE_AFMA6_DATA
99 
100 const char *const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
101 const char *const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
102 const char *const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
103 const char *const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
104 
106 
107 const unsigned int vpAfma6::njoint = 6;
108 
115  : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(vpAfma6::defaultTool),
116  projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
117 {
118  // Set the default parameters in case of the config files are not available.
119 
120  //
121  // Geometric model constant parameters
122  //
123  // coupling between join 5 and 6
124  this->_coupl_56 = 0.009091;
125  // distance between join 5 and 6
126  this->_long_56 = -0.06924;
127  // Camera extrinsic parameters: effector to camera frame
128  this->_eMc.eye(); // Default values are initialized ...
129  // ... in init (vpAfma6::vpAfma6ToolType tool,
130  // vpCameraParameters::vpCameraParametersProjType projModel)
131  // Maximal value of the joints
132  this->_joint_max[0] = 0.7001;
133  this->_joint_max[1] = 0.5201;
134  this->_joint_max[2] = 0.4601;
135  this->_joint_max[3] = 2.7301;
136  this->_joint_max[4] = 2.4801;
137  this->_joint_max[5] = 1.5901;
138  // Minimal value of the joints
139  this->_joint_min[0] = -0.6501;
140  this->_joint_min[1] = -0.6001;
141  this->_joint_min[2] = -0.5001;
142  this->_joint_min[3] = -2.7301;
143  this->_joint_min[4] = -0.1001;
144  this->_joint_min[5] = -1.5901;
145 
146  init();
147 }
148 
153 void vpAfma6::init(void)
154 {
155  this->init(vpAfma6::defaultTool);
156  return;
157 }
158 
171 void vpAfma6::init(const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters)
172 {
173  this->parseConfigFile(camera_extrinsic_parameters);
174 
175  this->parseConfigFile(camera_intrinsic_parameters);
176 }
177 
208 void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
209 {
210  this->setToolType(tool);
211  this->parseConfigFile(filename.c_str());
212 }
213 
223 void vpAfma6::init(const std::string &camera_extrinsic_parameters)
224 {
225  this->parseConfigFile(camera_extrinsic_parameters);
226 }
227 
244 {
245  this->setToolType(tool);
246  this->set_eMc(eMc);
247 }
248 
267 {
268 
269  this->projModel = proj_model;
270 
271 #ifdef VISP_HAVE_AFMA6_DATA
272  // Read the robot parameters from files
273  std::string filename_eMc;
274  switch (tool) {
275  case vpAfma6::TOOL_CCMOP: {
276  switch (projModel) {
279  break;
282  break;
283  }
284  break;
285  }
286  case vpAfma6::TOOL_GRIPPER: {
287  switch (projModel) {
290  break;
293  break;
294  }
295  break;
296  }
297  case vpAfma6::TOOL_VACUUM: {
298  switch (projModel) {
301  break;
304  break;
305  }
306  break;
307  }
309  switch (projModel) {
312  break;
315  break;
316  }
317  break;
318  }
319  default: {
320  vpERROR_TRACE("This error should not occur!");
321  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
322  // "que les specs de la classe ont ete modifiee, "
323  // "et que le code n'a pas ete mis a jour "
324  // "correctement.");
325  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
326  // "vpAfma6::vpAfma6ToolType, et controlez que "
327  // "tous les cas ont ete pris en compte dans la "
328  // "fonction init(camera).");
329  break;
330  }
331  }
332 
333  this->init(vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
334 
335 #else // VISP_HAVE_AFMA6_DATA
336 
337  // Use here default values of the robot constant parameters.
338  switch (tool) {
339  case vpAfma6::TOOL_CCMOP: {
340  switch (projModel) {
342  _erc[0] = vpMath::rad(164.35); // rx
343  _erc[1] = vpMath::rad(89.64); // ry
344  _erc[2] = vpMath::rad(-73.05); // rz
345  _etc[0] = 0.0117; // tx
346  _etc[1] = 0.0033; // ty
347  _etc[2] = 0.2272; // tz
348  break;
350  _erc[0] = vpMath::rad(33.54); // rx
351  _erc[1] = vpMath::rad(89.34); // ry
352  _erc[2] = vpMath::rad(57.83); // rz
353  _etc[0] = 0.0373; // tx
354  _etc[1] = 0.0024; // ty
355  _etc[2] = 0.2286; // tz
356  break;
357  }
358  break;
359  }
360  case vpAfma6::TOOL_GRIPPER: {
361  switch (projModel) {
363  _erc[0] = vpMath::rad(88.33); // rx
364  _erc[1] = vpMath::rad(72.07); // ry
365  _erc[2] = vpMath::rad(2.53); // rz
366  _etc[0] = 0.0783; // tx
367  _etc[1] = 0.1234; // ty
368  _etc[2] = 0.1638; // tz
369  break;
371  _erc[0] = vpMath::rad(86.69); // rx
372  _erc[1] = vpMath::rad(71.93); // ry
373  _erc[2] = vpMath::rad(4.17); // rz
374  _etc[0] = 0.1034; // tx
375  _etc[1] = 0.1142; // ty
376  _etc[2] = 0.1642; // tz
377  break;
378  }
379  break;
380  }
381  case vpAfma6::TOOL_VACUUM: {
382  switch (projModel) {
384  _erc[0] = vpMath::rad(90.40); // rx
385  _erc[1] = vpMath::rad(75.11); // ry
386  _erc[2] = vpMath::rad(0.18); // rz
387  _etc[0] = 0.0038; // tx
388  _etc[1] = 0.1281; // ty
389  _etc[2] = 0.1658; // tz
390  break;
392  _erc[0] = vpMath::rad(91.61); // rx
393  _erc[1] = vpMath::rad(76.17); // ry
394  _erc[2] = vpMath::rad(-0.98); // rz
395  _etc[0] = 0.0815; // tx
396  _etc[1] = 0.1162; // ty
397  _etc[2] = 0.1658; // tz
398  break;
399  }
400  break;
401  }
404  switch (projModel) {
407  // set eMc to identity
408  _erc[0] = 0; // rx
409  _erc[1] = 0; // ry
410  _erc[2] = 0; // rz
411  _etc[0] = 0; // tx
412  _etc[1] = 0; // ty
413  _etc[2] = 0; // tz
414  break;
415  }
416  break;
417  }
418  }
419  vpRotationMatrix eRc(_erc);
420  this->_eMc.buildFrom(_etc, eRc);
421 #endif // VISP_HAVE_AFMA6_DATA
422 
423  setToolType(tool);
424  return;
425 }
426 
452 {
454  fMc = get_fMc(q);
455 
456  return fMc;
457 }
458 
532 int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest,
533  const bool &verbose) const
534 {
536  double q_[2][6], d[2], t;
537  int ok[2];
538  double cord[6];
539 
540  int nbsol = 0;
541 
542  if (q.getRows() != njoint)
543  q.resize(6);
544 
545  // for(unsigned int i=0;i<3;i++) {
546  // fMe[i][3] = fMc[i][3];
547  // for(int j=0;j<3;j++) {
548  // fMe[i][j] = 0.0;
549  // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
550  // fMe[i][3] -= fMe[i][j]*rpi[j][3];
551  // }
552  // }
553 
554  // std::cout << "\n\nfMc: " << fMc;
555  // std::cout << "\n\neMc: " << _eMc;
556 
557  fMe = fMc * this->_eMc.inverse();
558  // std::cout << "\n\nfMe: " << fMe;
559 
560  if (fMe[2][2] >= .99999f) {
561  vpTRACE("singularity\n");
562  q_[0][4] = q_[1][4] = M_PI / 2.f;
563  t = atan2(fMe[0][0], fMe[0][1]);
564  q_[1][3] = q_[0][3] = q[3];
565  q_[1][5] = q_[0][5] = t - q_[0][3];
566 
567  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
568  /* -> a cause du couplage 4/5 */
569  {
570  q_[1][5] -= vpMath::rad(10);
571  q_[1][3] += vpMath::rad(10);
572  }
573  while (q_[1][5] <= this->_joint_min[5]) {
574  q_[1][5] += vpMath::rad(10);
575  q_[1][3] -= vpMath::rad(10);
576  }
577  } else if (fMe[2][2] <= -.99999) {
578  vpTRACE("singularity\n");
579  q_[0][4] = q_[1][4] = -M_PI / 2;
580  t = atan2(fMe[1][1], fMe[1][0]);
581  q_[1][3] = q_[0][3] = q[3];
582  q_[1][5] = q_[0][5] = q_[0][3] - t;
583  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
584  /* -> a cause du couplage 4/5 */
585  {
586  q_[1][5] -= vpMath::rad(10);
587  q_[1][3] -= vpMath::rad(10);
588  }
589  while (q_[1][5] <= this->_joint_min[5]) {
590  q_[1][5] += vpMath::rad(10);
591  q_[1][3] += vpMath::rad(10);
592  }
593  } else {
594  q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
595  if (q_[0][3] >= 0.0)
596  q_[1][3] = q_[0][3] - M_PI;
597  else
598  q_[1][3] = q_[0][3] + M_PI;
599 
600  q_[0][4] = asin(fMe[2][2]);
601  if (q_[0][4] >= 0.0)
602  q_[1][4] = M_PI - q_[0][4];
603  else
604  q_[1][4] = -M_PI - q_[0][4];
605 
606  q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
607  if (q_[0][5] >= 0.0)
608  q_[1][5] = q_[0][5] - M_PI;
609  else
610  q_[1][5] = q_[0][5] + M_PI;
611  }
612  q_[0][0] = fMe[0][3] - this->_long_56 * cos(q_[0][3]);
613  q_[1][0] = fMe[0][3] - this->_long_56 * cos(q_[1][3]);
614  q_[0][1] = fMe[1][3] - this->_long_56 * sin(q_[0][3]);
615  q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]);
616  q_[0][2] = q_[1][2] = fMe[2][3];
617 
618  /* prise en compte du couplage axes 5/6 */
619  q_[0][5] += this->_coupl_56 * q_[0][4];
620  q_[1][5] += this->_coupl_56 * q_[1][4];
621 
622  for (int j = 0; j < 2; j++) {
623  ok[j] = 1;
624  // test is position is reachable
625  for (unsigned int i = 0; i < 6; i++) {
626  if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
627  if (verbose) {
628  if (i < 3)
629  std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < "
630  << this->_joint_max[i] << std::endl;
631  else
632  std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < "
633  << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
634  }
635  ok[j] = 0;
636  }
637  }
638  }
639  if (ok[0] == 0) {
640  if (ok[1] == 0) {
641  std::cout << "No solution..." << std::endl;
642  nbsol = 0;
643  return nbsol;
644  } else if (ok[1] == 1) {
645  for (unsigned int i = 0; i < 6; i++)
646  cord[i] = q_[1][i];
647  nbsol = 1;
648  }
649  } else {
650  if (ok[1] == 0) {
651  for (unsigned int i = 0; i < 6; i++)
652  cord[i] = q_[0][i];
653  nbsol = 1;
654  } else {
655  nbsol = 2;
656  // vpTRACE("2 solutions\n");
657  for (int j = 0; j < 2; j++) {
658  d[j] = 0.0;
659  for (unsigned int i = 3; i < 6; i++)
660  d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
661  }
662  if (nearest == true) {
663  if (d[0] <= d[1])
664  for (unsigned int i = 0; i < 6; i++)
665  cord[i] = q_[0][i];
666  else
667  for (unsigned int i = 0; i < 6; i++)
668  cord[i] = q_[1][i];
669  } else {
670  if (d[0] <= d[1])
671  for (unsigned int i = 0; i < 6; i++)
672  cord[i] = q_[1][i];
673  else
674  for (unsigned int i = 0; i < 6; i++)
675  cord[i] = q_[0][i];
676  }
677  }
678  }
679  for (unsigned int i = 0; i < 6; i++)
680  q[i] = cord[i];
681 
682  return nbsol;
683 }
684 
708 {
710  get_fMc(q, fMc);
711 
712  return fMc;
713 }
714 
735 {
736 
737  // Compute the direct geometric model: fMe = transformation between
738  // fix and end effector frame.
740 
741  get_fMe(q, fMe);
742 
743  fMc = fMe * this->_eMc;
744 
745  return;
746 }
747 
768 {
769  double q0 = q[0]; // meter
770  double q1 = q[1]; // meter
771  double q2 = q[2]; // meter
772 
773  /* Decouplage liaisons 2 et 3. */
774  double q5 = q[5] - this->_coupl_56 * q[4];
775 
776  double c1 = cos(q[3]);
777  double s1 = sin(q[3]);
778  double c2 = cos(q[4]);
779  double s2 = sin(q[4]);
780  double c3 = cos(q5);
781  double s3 = sin(q5);
782 
783  // Compute the direct geometric model: fMe = transformation betwee
784  // fix and end effector frame.
785  fMe[0][0] = s1 * s2 * c3 + c1 * s3;
786  fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
787  fMe[0][2] = -s1 * c2;
788  fMe[0][3] = q0 + this->_long_56 * c1;
789 
790  fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
791  fMe[1][1] = c1 * s2 * s3 + s1 * c3;
792  fMe[1][2] = c1 * c2;
793  fMe[1][3] = q1 + this->_long_56 * s1;
794 
795  fMe[2][0] = c2 * c3;
796  fMe[2][1] = -c2 * s3;
797  fMe[2][2] = s2;
798  fMe[2][3] = q2;
799 
800  fMe[3][0] = 0;
801  fMe[3][1] = 0;
802  fMe[3][2] = 0;
803  fMe[3][3] = 1;
804 
805  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
806 
807  return;
808 }
809 
820 void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
831 vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
832 
843 {
845  get_cMe(cMe);
846 
847  cVe.buildFrom(cMe);
848 
849  return;
850 }
851 
864 void vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
865 {
866 
867  eJe.resize(6, 6);
868 
869  double s4, c4, s5, c5, s6, c6;
870 
871  s4 = sin(q[3]);
872  c4 = cos(q[3]);
873  s5 = sin(q[4]);
874  c5 = cos(q[4]);
875  s6 = sin(q[5] - this->_coupl_56 * q[4]);
876  c6 = cos(q[5] - this->_coupl_56 * q[4]);
877 
878  eJe = 0;
879  eJe[0][0] = s4 * s5 * c6 + c4 * s6;
880  eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
881  eJe[0][2] = c5 * c6;
882  eJe[0][3] = -this->_long_56 * s5 * c6;
883 
884  eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
885  eJe[1][1] = c4 * s5 * s6 + s4 * c6;
886  eJe[1][2] = -c5 * s6;
887  eJe[1][3] = this->_long_56 * s5 * s6;
888 
889  eJe[2][0] = -s4 * c5;
890  eJe[2][1] = c4 * c5;
891  eJe[2][2] = s5;
892  eJe[2][3] = this->_long_56 * c5;
893 
894  eJe[3][3] = c5 * c6;
895  eJe[3][4] = s6;
896 
897  eJe[4][3] = -c5 * s6;
898  eJe[4][4] = c6;
899 
900  eJe[5][3] = s5;
901  eJe[5][4] = -this->_coupl_56;
902  eJe[5][5] = 1;
903 
904  return;
905 }
906 
934 void vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
935 {
936 
937  fJe.resize(6, 6);
938 
939  // block superieur gauche
940  fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
941 
942  double s4 = sin(q[3]);
943  double c4 = cos(q[3]);
944 
945  // block superieur droit
946  fJe[0][3] = -this->_long_56 * s4;
947  fJe[1][3] = this->_long_56 * c4;
948 
949  double s5 = sin(q[4]);
950  double c5 = cos(q[4]);
951  // block inferieur droit
952  fJe[3][4] = c4;
953  fJe[3][5] = -s4 * c5;
954  fJe[4][4] = s4;
955  fJe[4][5] = c4 * c5;
956  fJe[5][3] = 1;
957  fJe[5][5] = s5;
958 
959  // coupling between joint 5 and 6
960  fJe[3][4] += this->_coupl_56 * s4 * c5;
961  fJe[4][4] += -this->_coupl_56 * c4 * c5;
962  fJe[5][4] += -this->_coupl_56 * s5;
963 
964  return;
965 }
966 
976 {
977  vpColVector qmin(6);
978  for (unsigned int i = 0; i < 6; i++)
979  qmin[i] = this->_joint_min[i];
980  return qmin;
981 }
982 
992 {
993  vpColVector qmax(6);
994  for (unsigned int i = 0; i < 6; i++)
995  qmax[i] = this->_joint_max[i];
996  return qmax;
997 }
998 
1005 double vpAfma6::getCoupl56() const { return _coupl_56; }
1006 
1013 double vpAfma6::getLong56() const { return _long_56; }
1014 
1025 void vpAfma6::parseConfigFile(const std::string &filename)
1026 {
1027  vpRxyzVector erc; // eMc rotation
1028  vpTranslationVector etc; // eMc translation
1029 
1030  std::ifstream fdconfig(filename.c_str(), std::ios::in);
1031 
1032  if (!fdconfig.is_open()) {
1033  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
1034  filename.c_str());
1035  }
1036 
1037  std::string line;
1038  int lineNum = 0;
1039  bool get_erc = false;
1040  bool get_etc = false;
1041  int code;
1042 
1043  while (std::getline(fdconfig, line)) {
1044  lineNum++;
1045  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
1046  continue;
1047  }
1048  std::istringstream ss(line);
1049  std::string key;
1050  ss >> key;
1051 
1052  for (code = 0; NULL != opt_Afma6[code]; ++code) {
1053  if (key.compare(opt_Afma6[code]) == 0) {
1054  break;
1055  }
1056  }
1057 
1058  switch (code) {
1059  case 0:
1060  ss >> this->_joint_max[0] >> this->_joint_max[1] >> this->_joint_max[2] >> this->_joint_max[3] >>
1061  this->_joint_max[4] >> this->_joint_max[5];
1062  break;
1063 
1064  case 1:
1065  ss >> this->_joint_min[0] >> this->_joint_min[1] >> this->_joint_min[2] >> this->_joint_min[3] >>
1066  this->_joint_min[4] >> this->_joint_min[5];
1067  break;
1068 
1069  case 2:
1070  ss >> this->_long_56;
1071  break;
1072 
1073  case 3:
1074  ss >> this->_coupl_56;
1075  break;
1076 
1077  case 4:
1078  break; // Nothing to do: camera name
1079 
1080  case 5:
1081  ss >> erc[0] >> erc[1] >> erc[2];
1082 
1083  // Convert rotation from degrees to radians
1084  erc = erc * M_PI / 180.0;
1085  get_erc = true;
1086  break;
1087 
1088  case 6:
1089  ss >> etc[0] >> etc[1] >> etc[2];
1090  get_etc = true;
1091  break;
1092 
1093  default:
1094  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
1095  filename.c_str(), lineNum));
1096  }
1097  }
1098 
1099  fdconfig.close();
1100 
1101  // Compute the eMc matrix from the translations and rotations
1102  if (get_etc && get_erc) {
1103  _erc = erc;
1104  _etc = etc;
1105 
1106  vpRotationMatrix eRc(_erc);
1107  this->_eMc.buildFrom(_etc, eRc);
1108  }
1109 }
1110 
1120 {
1121  this->_eMc = eMc;
1122  this->_etc = eMc.getTranslationVector();
1123 
1124  vpRotationMatrix R(eMc);
1125  this->_erc.buildFrom(R);
1126 }
1127 
1190 void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
1191  const unsigned int &image_height) const
1192 {
1193 #if defined(VISP_HAVE_PUGIXML) && defined(VISP_HAVE_AFMA6_DATA)
1194  vpXmlParserCamera parser;
1195  switch (getToolType()) {
1196  case vpAfma6::TOOL_CCMOP: {
1197  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1198  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1200  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1201  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1202  }
1203  break;
1204  }
1205  case vpAfma6::TOOL_GRIPPER: {
1206  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1207  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1209  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1210  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1211  }
1212  break;
1213  }
1214  case vpAfma6::TOOL_VACUUM: {
1215  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1216  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1218  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1219  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1220  }
1221  break;
1222  }
1224  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1225  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1227  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1228  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1229  }
1230  break;
1231  }
1232  default: {
1233  vpERROR_TRACE("This error should not occur!");
1234  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1235  // "que les specs de la classe ont ete modifiee, "
1236  // "et que le code n'a pas ete mis a jour "
1237  // "correctement.");
1238  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1239  // "vpAfma6::vpAfma6ToolType, et controlez que "
1240  // "tous les cas ont ete pris en compte dans la "
1241  // "fonction init(camera).");
1242  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1243  }
1244  }
1245 #else
1246  // Set default parameters
1247  switch (getToolType()) {
1248  case vpAfma6::TOOL_CCMOP: {
1249  // Set default intrinsic camera parameters for 640x480 images
1250  if (image_width == 640 && image_height == 480) {
1251  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
1252  << std::endl;
1253  switch (this->projModel) {
1255  cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1256  break;
1258  cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1259  break;
1260  }
1261  } else {
1262  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1263  "resolution");
1264  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1265  }
1266  break;
1267  }
1268  case vpAfma6::TOOL_GRIPPER: {
1269  // Set default intrinsic camera parameters for 640x480 images
1270  if (image_width == 640 && image_height == 480) {
1271  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
1272  << std::endl;
1273  switch (this->projModel) {
1275  cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
1276  break;
1278  cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
1279  break;
1280  }
1281  } else {
1282  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1283  "resolution");
1284  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1285  }
1286  break;
1287  }
1288  case vpAfma6::TOOL_VACUUM: {
1289  // Set default intrinsic camera parameters for 640x480 images
1290  if (image_width == 640 && image_height == 480) {
1291  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\""
1292  << std::endl;
1293  switch (this->projModel) {
1295  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1296  break;
1298  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1299  break;
1300  }
1301  } else {
1302  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1303  "resolution");
1304  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1305  }
1306  break;
1307  }
1309  // Set default intrinsic camera parameters for 640x480 images
1310  if (image_width == 640 && image_height == 480) {
1311  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\""
1312  << std::endl;
1313  switch (this->projModel) {
1315  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1316  break;
1318  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1319  break;
1320  }
1321  } else {
1322  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1323  "resolution");
1324  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1325  }
1326  break;
1327  }
1328  default:
1329  vpERROR_TRACE("This error should not occur!");
1330  break;
1331  }
1332 #endif
1333  return;
1334 }
1335 
1379 {
1380  getCameraParameters(cam, I.getWidth(), I.getHeight());
1381 }
1427 {
1428  getCameraParameters(cam, I.getWidth(), I.getHeight());
1429 }
1430 
1440 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6)
1441 {
1442  vpRotationMatrix eRc;
1443  afma6._eMc.extract(eRc);
1444  vpRxyzVector rxyz(eRc);
1445 
1446  os << "Joint Max:" << std::endl
1447  << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t"
1448  << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl
1449 
1450  << "Joint Min: " << std::endl
1451  << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t"
1452  << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl
1453 
1454  << "Long 5-6: " << std::endl
1455  << "\t" << afma6._long_56 << "\t" << std::endl
1456 
1457  << "Coupling 5-6:" << std::endl
1458  << "\t" << afma6._coupl_56 << "\t" << std::endl
1459 
1460  << "eMc: " << std::endl
1461  << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t"
1462  << std::endl
1463  << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1464  << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1465  << "\t" << std::endl;
1466 
1467  return os;
1468 }
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:92
Modelisation of Irisa&#39;s gantry robot named Afma6.
Definition: vpAfma6.h:78
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpRxyzVector buildFrom(const vpRotationMatrix &R)
vpRxyzVector _erc
Definition: vpAfma6.h:198
vpTranslationVector _etc
Definition: vpAfma6.h:197
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1190
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:185
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:842
static const std::string CONST_AFMA6_FILENAME
Definition: vpAfma6.h:85
Error that can be emited by the vpRobot class and its derivates.
unsigned int getWidth() const
Definition: vpImage.h:237
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:88
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma6.cpp:451
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:532
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:160
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:707
#define vpERROR_TRACE
Definition: vpDebug.h:393
void parseConfigFile(const std::string &filename)
Definition: vpAfma6.cpp:1025
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:87
double _coupl_56
Definition: vpAfma6.h:192
static const std::string CONST_CAMERA_AFMA6_FILENAME
Definition: vpAfma6.h:94
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:864
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:118
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:206
XML parser to load and save intrinsic camera parameters.
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma6.cpp:767
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpAfma6 &afma6)
Definition: vpAfma6.cpp:1440
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:185
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:93
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:105
Implementation of a rotation matrix and operations on such kind of matrices.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:127
vpColVector getJointMin() const
Definition: vpAfma6.cpp:975
vpHomogeneousMatrix get_eMc() const
Definition: vpAfma6.cpp:831
vpTranslationVector getTranslationVector() const
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:90
#define vpTRACE
Definition: vpDebug.h:416
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:91
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:820
Generic class defining intrinsic camera parameters.
vpColVector getJointMax() const
Definition: vpAfma6.cpp:991
void extract(vpRotationMatrix &R) const
unsigned int getRows() const
Definition: vpArray2D.h:289
vpAfma6()
Definition: vpAfma6.cpp:114
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
Definition: vpMath.h:108
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:86
double getLong56() const
Definition: vpAfma6.cpp:1013
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1119
double _long_56
Definition: vpAfma6.h:193
static double deg(double rad)
Definition: vpMath.h:101
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:200
unsigned int getHeight() const
Definition: vpImage.h:179
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
void init(void)
Definition: vpAfma6.cpp:153
double _joint_max[6]
Definition: vpAfma6.h:194
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:100
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:89
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:934
static const char *const CONST_VACUUM_CAMERA_NAME
Definition: vpAfma6.h:110
Class that consider the case of a translation vector.
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
double _joint_min[6]
Definition: vpAfma6.h:195
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpAfma6.h:115
double getCoupl56() const
Definition: vpAfma6.cpp:1005