Visual Servoing Platform  version 3.2.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 
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 
734 void vpAfma6::get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
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 
767 void vpAfma6::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
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 
1119 void vpAfma6::set_eMc(const vpHomogeneousMatrix &eMc)
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_XML2) && 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 }
vpAfma6::_etc
vpTranslationVector _etc
Definition: vpAfma6.h:196
vpAfma6::getForwardKinematics
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma6.cpp:450
vpXmlParserCamera::parse
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)
Definition: vpXmlParserCamera.cpp:137
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:243
vpAfma6::init
void init(void)
Definition: vpAfma6.cpp:152
vpAfma6::get_cVe
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:841
vpAfma6::_long_56
double _long_56
Definition: vpAfma6.h:192
vpAfma6::CONST_AFMA6_FILENAME
static const std::string CONST_AFMA6_FILENAME
Definition: vpAfma6.h:84
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:101
vpAfma6::vpAfma6
vpAfma6()
Definition: vpAfma6.cpp:113
vpAfma6::getJointMax
vpColVector getJointMax() const
Definition: vpAfma6.cpp:990
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpCameraParameters::perspectiveProjWithoutDistortion
Definition: vpCameraParameters.h:239
vpAfma6::projModel
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:205
vpXmlParserCamera
XML parser to load and save intrinsic camera parameters.
Definition: vpXmlParserCamera.h:187
vpAfma6::TOOL_VACUUM
Definition: vpAfma6.h:120
vpAfma6::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:91
vpMath::deg
static double deg(double rad)
Definition: vpMath.h:94
vpAfma6::CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:86
vpRobotException
Error that can be emited by the vpRobot class and its derivates.
Definition: vpRobotException.h:70
vpAfma6::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:92
vpRobotException::readingParametersError
Definition: vpRobotException.h:107
vpAfma6::get_fJe
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:933
vpImageSimulator::init
void init(const vpImage< unsigned char > &I, vpColVector *X)
Definition: vpImageSimulator.cpp:1080
vpAfma6::CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:90
vpAfma6::getInverseKinematics
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:531
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:90
vpAfma6::CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:87
vpAfma6::CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:88
vpAfma6::getCoupl56
double getCoupl56() const
Definition: vpAfma6.cpp:1004
vpCameraParameters::vpCameraParametersProjType
vpCameraParametersProjType
Definition: vpCameraParameters.h:238
vpAfma6::TOOL_GRIPPER
Definition: vpAfma6.h:119
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:71
vpHomogeneousMatrix::extract
void extract(vpRotationMatrix &R) const
Definition: vpHomogeneousMatrix.cpp:552
vpAfma6::_joint_max
double _joint_max[6]
Definition: vpAfma6.h:193
vpAfma6::TOOL_CUSTOM
Definition: vpAfma6.h:122
vpAfma6::_joint_min
double _joint_min[6]
Definition: vpAfma6.h:194
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:103
vpAfma6::CONST_CCMOP_CAMERA_NAME
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:99
vpHomogeneousMatrix::getTranslationVector
vpTranslationVector getTranslationVector() const
Definition: vpHomogeneousMatrix.cpp:786
vpAfma6
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:77
vpAfma6::CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:85
vpAfma6::getCameraParameters
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1189
vpRotationMatrix
Implementation of a rotation matrix and operations on such kind of matrices.
Definition: vpRotationMatrix.h:70
vpAfma6::CONST_GENERIC_CAMERA_NAME
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpAfma6.h:114
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:177
vpAfma6::_erc
vpRxyzVector _erc
Definition: vpAfma6.h:197
vpAfma6::CONST_VACUUM_CAMERA_NAME
static const char *const CONST_VACUUM_CAMERA_NAME
Definition: vpAfma6.h:109
vpAfma6::CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:89
vpAfma6::_eMc
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:199
vpAfma6::get_fMe
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma6.cpp:766
vpAfma6::CONST_CAMERA_AFMA6_FILENAME
static const std::string CONST_CAMERA_AFMA6_FILENAME
Definition: vpAfma6.h:93
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:113
vpAfma6::parseConfigFile
void parseConfigFile(const std::string &filename)
Definition: vpAfma6.cpp:1024
vpAfma6::defaultTool
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:126
vpHomogeneousMatrix::buildFrom
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition: vpHomogeneousMatrix.cpp:222
vpAfma6::TOOL_GENERIC_CAMERA
Definition: vpAfma6.h:121
vpAfma6::vpAfma6ToolType
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:117
vpVelocityTwistMatrix::buildFrom
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition: vpVelocityTwistMatrix.cpp:364
vpAfma6::get_cMe
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:819
vpERROR_TRACE
#define vpERROR_TRACE
Definition: vpDebug.h:392
vpAfma6::getLong56
double getLong56() const
Definition: vpAfma6.cpp:1012
vpAfma6::get_eJe
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:863
vpCameraParameters::initPersProjWithDistortion
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
Definition: vpCameraParameters.cpp:248
vpAfma6::njoint
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:184
vpAfma6::_coupl_56
double _coupl_56
Definition: vpAfma6.h:191
vpImage< unsigned char >
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:642
vpArray2D::resize
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:170
vpRxyzVector::buildFrom
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Definition: vpRxyzVector.cpp:101
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:91
vpAfma6::setToolType
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:184
vpAfma6::TOOL_CCMOP
Definition: vpAfma6.h:118
vpAfma6::get_eMc
vpHomogeneousMatrix get_eMc() const
Definition: vpAfma6.cpp:830
vpCameraParameters::perspectiveProjWithDistortion
Definition: vpCameraParameters.h:241
vpAfma6::getToolType
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:159
vpTRACE
#define vpTRACE
Definition: vpDebug.h:415
vpAfma6::getJointMin
vpColVector getJointMin() const
Definition: vpAfma6.cpp:974
vpAfma6::get_fMc
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:706
vpXmlParserCamera::SEQUENCE_OK
Definition: vpXmlParserCamera.h:215
vpRxyzVector
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:155
vpCameraParameters::initPersProjWithoutDistortion
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
Definition: vpCameraParameters.cpp:182
vpAfma6::CONST_GRIPPER_CAMERA_NAME
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:104
vpArray2D::operator<<
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:274
vpAfma6::set_eMc
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1118
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:238
vpArray2D::getRows
unsigned int getRows() const
Definition: vpArray2D.h:155