ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpAfma6.cpp
1 /****************************************************************************
2  *
3  * $Id: vpAfma6.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface for the Irisa's Afma6 robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
51 #include <visp/vpDebug.h>
52 #include <visp/vpVelocityTwistMatrix.h>
53 #include <visp/vpRobotException.h>
54 #include <visp/vpXmlParserCamera.h>
55 #include <visp/vpCameraParameters.h>
56 #include <visp/vpRxyzVector.h>
57 #include <visp/vpTranslationVector.h>
58 #include <visp/vpRotationMatrix.h>
59 #include <visp/vpAfma6.h>
60 
61 /* ----------------------------------------------------------------------- */
62 /* --- STATIC ------------------------------------------------------------ */
63 /* ---------------------------------------------------------------------- */
64 
65 #ifdef VISP_HAVE_ACCESS_TO_NAS
66 static const char *opt_Afma6[] = {"JOINT_MAX","JOINT_MIN","LONG_56","COUPL_56",
67  "CAMERA", "eMc_ROT_XYZ","eMc_TRANS_XYZ",
68  NULL};
69 
70 const char * const vpAfma6::CONST_AFMA6_FILENAME
71 #ifdef WIN32
72 = "Z:/robot/Afma6/current/include/const_Afma6.cnf";
73 #else
74 = "/udd/fspindle/robot/Afma6/current/include/const_Afma6.cnf";
75 #endif
76 
78 #ifdef WIN32
79 = "Z:/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf";
80 #else
81 = "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf";
82 #endif
83 
85 #ifdef WIN32
86 = "Z:/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf";
87 #else
88 = "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf";
89 #endif
90 
92 #ifdef WIN32
93 = "Z:/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf";
94 #else
95 = "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf";
96 #endif
97 
99 #ifdef WIN32
100 = "Z:/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf";
101 #else
102 = "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf";
103 #endif
104 
106 #ifdef WIN32
107 = "Z:/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf";
108 #else
109 = "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf";
110 #endif
111 
113 #ifdef WIN32
114 = "Z:/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf";
115 #else
116 = "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf";
117 #endif
118 
120 #ifdef WIN32
121 = "Z:/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf";
122 #else
123 = "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf";
124 #endif
125 
127 #ifdef WIN32
128 = "Z:/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf";
129 #else
130 = "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf";
131 #endif
132 
133 const char * const vpAfma6::CONST_CAMERA_AFMA6_FILENAME
134 #ifdef WIN32
135 = "Z:/robot/Afma6/current/include/const_camera_Afma6.xml";
136 #else
137 = "/udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml";
138 #endif
139 
140 #endif // VISP_HAVE_ACCESS_TO_NAS
141 
142 const char * const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
143 const char * const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
144 const char * const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
145 const char * const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
146 
148 
149 const unsigned int vpAfma6::njoint = 6;
150 
157 {
158  // Set the default parameters in case of the config files on the NAS
159  // at Inria are not available.
160 
161  //
162  // Geometric model constant parameters
163  //
164  // coupling between join 5 and 6
165  this->_coupl_56 = 0.009091;
166  // distance between join 5 and 6
167  this->_long_56 = -0.06924;
168  // Camera extrinsic parameters: effector to camera frame
169  this->_eMc.setIdentity(); // Default values are initialized ...
170  // ... in init (vpAfma6::vpAfma6ToolType tool,
171  // vpCameraParameters::vpCameraParametersProjType projModel)
172  // Maximal value of the joints
173  this->_joint_max[0] = 0.7001;
174  this->_joint_max[1] = 0.5201;
175  this->_joint_max[2] = 0.4601;
176  this->_joint_max[3] = 2.7301;
177  this->_joint_max[4] = 2.4801;
178  this->_joint_max[5] = 1.5901;
179  // Minimal value of the joints
180  this->_joint_min[0] = -0.6501;
181  this->_joint_min[1] = -0.6001;
182  this->_joint_min[2] = -0.5001;
183  this->_joint_min[3] = -2.7301;
184  this->_joint_min[4] = -0.1001;
185  this->_joint_min[5] = -1.5901;
186 
187  init();
188 
189 }
190 
195 void
197 {
198  this->init ( vpAfma6::defaultTool);
199  return;
200 }
201 
216 #ifdef VISP_HAVE_ACCESS_TO_NAS
217 void
218 vpAfma6::init (const char * paramAfma6,
219  const char * paramCamera)
220 {
221  // vpTRACE ("Parsage fichier robot.");
222  this->parseConfigFile (paramAfma6);
223 
224  //vpTRACE ("Parsage fichier camera.");
225  this->parseConfigFile (paramCamera);
226 
227  return ;
228 }
229 #endif
230 
243 void
246 {
247 
248  this->projModel = projModel;
249 
250 #ifdef VISP_HAVE_ACCESS_TO_NAS
251  // Read the robot parameters from files
252  char filename_eMc [FILENAME_MAX];
253  switch (tool) {
254  case vpAfma6::TOOL_CCMOP: {
255  switch(projModel) {
257 #ifdef UNIX
258  snprintf(filename_eMc, FILENAME_MAX, "%s",
260 #else // WIN32
261  _snprintf(filename_eMc, FILENAME_MAX, "%s",
263 #endif
264  break;
266 #ifdef UNIX
267  snprintf(filename_eMc, FILENAME_MAX, "%s",
269 #else // WIN32
270  _snprintf(filename_eMc, FILENAME_MAX, "%s",
272 #endif
273  break;
274  }
275  break;
276  }
277  case vpAfma6::TOOL_GRIPPER: {
278  switch(projModel) {
280 #ifdef UNIX
281  snprintf(filename_eMc, FILENAME_MAX, "%s",
283 #else // WIN32
284  _snprintf(filename_eMc, FILENAME_MAX, "%s",
286 #endif
287  break;
289 #ifdef UNIX
290  snprintf(filename_eMc, FILENAME_MAX, "%s",
292 #else // WIN32
293  _snprintf(filename_eMc, FILENAME_MAX, "%s",
295 #endif
296  break;
297  }
298  break;
299  }
300  case vpAfma6::TOOL_VACUUM: {
301  switch(projModel) {
303 #ifdef UNIX
304  snprintf(filename_eMc, FILENAME_MAX, "%s",
306 #else // WIN32
307  _snprintf(filename_eMc, FILENAME_MAX, "%s",
309 #endif
310  break;
312 #ifdef UNIX
313  snprintf(filename_eMc, FILENAME_MAX, "%s",
315 #else // WIN32
316  _snprintf(filename_eMc, FILENAME_MAX, "%s",
318 #endif
319  break;
320  }
321  break;
322  }
324  switch(projModel) {
326 #ifdef UNIX
327  snprintf(filename_eMc, FILENAME_MAX, "%s",
329 #else // WIN32
330  _snprintf(filename_eMc, FILENAME_MAX, "%s",
332 #endif
333  break;
335 #ifdef UNIX
336  snprintf(filename_eMc, FILENAME_MAX, "%s",
338 #else // WIN32
339  _snprintf(filename_eMc, FILENAME_MAX, "%s",
341 #endif
342  break;
343  }
344  break;
345  }
346  default: {
347  vpERROR_TRACE ("This error should not occur!");
348  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
349  // "que les specs de la classe ont ete modifiee, "
350  // "et que le code n'a pas ete mis a jour "
351  // "correctement.");
352  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
353  // "vpAfma6::vpAfma6ToolType, et controlez que "
354  // "tous les cas ont ete pris en compte dans la "
355  // "fonction init(camera).");
356  break;
357  }
358  }
359 
360  this->init (vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
361 
362 #else // VISP_HAVE_ACCESS_TO_NAS
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;
384  }
385  }
386  case vpAfma6::TOOL_GRIPPER: {
387  switch(projModel) {
389  _erc[0] = vpMath::rad( 88.33); // rx
390  _erc[1] = vpMath::rad( 72.07); // ry
391  _erc[2] = vpMath::rad( 2.53); // rz
392  _etc[0] = 0.0783; // tx
393  _etc[1] = 0.1234; // ty
394  _etc[2] = 0.1638; // tz
395  break;
397  _erc[0] = vpMath::rad(86.69); // rx
398  _erc[1] = vpMath::rad(71.93); // ry
399  _erc[2] = vpMath::rad( 4.17); // rz
400  _etc[0] = 0.1034; // tx
401  _etc[1] = 0.1142; // ty
402  _etc[2] = 0.1642; // tz
403  break;
404  }
405  }
406  case vpAfma6::TOOL_VACUUM: {
407  switch(projModel) {
409  _erc[0] = vpMath::rad( 90.40); // rx
410  _erc[1] = vpMath::rad( 75.11); // ry
411  _erc[2] = vpMath::rad( 0.18); // rz
412  _etc[0] = 0.0038; // tx
413  _etc[1] = 0.1281; // ty
414  _etc[2] = 0.1658; // tz
415  break;
417  _erc[0] = vpMath::rad(91.61); // rx
418  _erc[1] = vpMath::rad(76.17); // ry
419  _erc[2] = vpMath::rad(-0.98); // rz
420  _etc[0] = 0.0815; // tx
421  _etc[1] = 0.1162; // ty
422  _etc[2] = 0.1658; // tz
423  break;
424  }
425  }
427  switch(projModel) {
430  // set eMc to identity
431  _erc[0] = 0; // rx
432  _erc[1] = 0; // ry
433  _erc[2] = 0; // rz
434  _etc[0] = 0; // tx
435  _etc[1] = 0; // ty
436  _etc[2] = 0; // tz
437  break;
438  }
439  }
440  }
441  vpRotationMatrix eRc(_erc);
442  this->_eMc.buildFrom(_etc, eRc);
443 #endif // VISP_HAVE_ACCESS_TO_NAS
444 
445  setToolType(tool);
446  return ;
447 }
448 
449 
476 {
478  fMc = get_fMc(q);
479 
480  return fMc;
481 }
482 
554 int
556  vpColVector & q, const bool &nearest)
557 {
559  double q_[2][6],d[2],t;
560  int ok[2];
561  double cord[6];
562 
563  int nbsol = 0;
564 
565  if (q.getRows() != njoint)
566  q.resize(6);
567 
568 
569  // for(unsigned int i=0;i<3;i++) {
570  // fMe[i][3] = fMc[i][3];
571  // for(int j=0;j<3;j++) {
572  // fMe[i][j] = 0.0;
573  // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
574  // fMe[i][3] -= fMe[i][j]*rpi[j][3];
575  // }
576  // }
577 
578  // std::cout << "\n\nfMc: " << fMc;
579  // std::cout << "\n\neMc: " << _eMc;
580 
581  fMe = fMc * this->_eMc.inverse();
582  // std::cout << "\n\nfMe: " << fMe;
583 
584  if (fMe[2][2] >= .99999f)
585  {
586  vpTRACE("singularity\n");
587  q_[0][4] = q_[1][4] = M_PI/2.f;
588  t = atan2(fMe[0][0],fMe[0][1]);
589  q_[1][3] = q_[0][3] = q[3];
590  q_[1][5] = q_[0][5] = t - q_[0][3];
591 
592  while ((q_[1][5]+vpMath::rad(2)) >= this->_joint_max[5])
593  /* -> a cause du couplage 4/5 */
594  {
595  q_[1][5] -= vpMath::rad(10);
596  q_[1][3] += vpMath::rad(10);
597  }
598  while (q_[1][5] <= this->_joint_min[5])
599  {
600  q_[1][5] += vpMath::rad(10);
601  q_[1][3] -= vpMath::rad(10);
602  }
603  }
604  else if (fMe[2][2] <= -.99999)
605  {
606  vpTRACE("singularity\n");
607  q_[0][4] = q_[1][4] = -M_PI/2;
608  t = atan2(fMe[1][1],fMe[1][0]);
609  q_[1][3] = q_[0][3] = q[3];
610  q_[1][5] = q_[0][5] = q_[0][3] - t;
611  while ((q_[1][5]+vpMath::rad(2)) >= this->_joint_max[5])
612  /* -> a cause du couplage 4/5 */
613  {
614  q_[1][5] -= vpMath::rad(10);
615  q_[1][3] -= vpMath::rad(10);
616  }
617  while (q_[1][5] <= this->_joint_min[5])
618  {
619  q_[1][5] += vpMath::rad(10);
620  q_[1][3] += vpMath::rad(10);
621  }
622  }
623  else
624  {
625  q_[0][3] = atan2(-fMe[0][2],fMe[1][2]);
626  if (q_[0][3] >= 0.0) q_[1][3] = q_[0][3] - M_PI;
627  else q_[1][3] = q_[0][3] + M_PI;
628 
629  q_[0][4] = asin(fMe[2][2]);
630  if (q_[0][4] >= 0.0) q_[1][4] = M_PI - q_[0][4];
631  else q_[1][4] = -M_PI - q_[0][4];
632 
633  q_[0][5] = atan2(-fMe[2][1],fMe[2][0]);
634  if (q_[0][5] >= 0.0) q_[1][5] = q_[0][5] - M_PI;
635  else q_[1][5] = q_[0][5] + M_PI;
636  }
637  q_[0][0] = fMe[0][3] - this->_long_56*cos(q_[0][3]);
638  q_[1][0] = fMe[0][3] - this->_long_56*cos(q_[1][3]);
639  q_[0][1] = fMe[1][3] - this->_long_56*sin(q_[0][3]);
640  q_[1][1] = fMe[1][3] - this->_long_56*sin(q_[1][3]);
641  q_[0][2] = q_[1][2] = fMe[2][3];
642 
643  /* prise en compte du couplage axes 5/6 */
644  q_[0][5] += this->_coupl_56*q_[0][4];
645  q_[1][5] += this->_coupl_56*q_[1][4];
646 
647  for (int j=0;j<2;j++)
648  {
649  ok[j] = 1;
650  // test is position is reachable
651  for (unsigned int i=0;i<6;i++) {
652  if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i])
653  ok[j] = 0;
654  }
655  }
656  if (ok[0] == 0)
657  {
658  if (ok[1] == 0) {
659  std::cout << "No solution..." << std::endl;
660  nbsol = 0;
661  return nbsol;
662  }
663  else if (ok[1] == 1) {
664  for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
665  nbsol = 1;
666  }
667  }
668  else
669  {
670  if (ok[1] == 0) {
671  for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
672  nbsol = 1;
673  }
674  else
675  {
676  nbsol = 2;
677  //vpTRACE("2 solutions\n");
678  for (int j=0;j<2;j++)
679  {
680  d[j] = 0.0;
681  for (unsigned int i=3;i<6;i++)
682  d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
683  }
684  if (nearest == true)
685  {
686  if (d[0] <= d[1])
687  for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
688  else
689  for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
690  }
691  else
692  {
693  if (d[0] <= d[1])
694  for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
695  else
696  for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
697  }
698  }
699  }
700  for(unsigned int i=0; i<6; i++)
701  q[i] = cord[i] ;
702 
703  return nbsol;
704 }
705 
730 {
732  get_fMc(q, fMc);
733 
734  return fMc;
735 }
736 
756 void
758 {
759 
760  // Compute the direct geometric model: fMe = transformation between
761  // fix and end effector frame.
763 
764  get_fMe(q, fMe);
765 
766  fMc = fMe * this->_eMc;
767 
768  return;
769 }
770 
790 void
792 {
793  double q0 = q[0]; // meter
794  double q1 = q[1]; // meter
795  double q2 = q[2]; // meter
796 
797  /* Decouplage liaisons 2 et 3. */
798  double q5 = q[5] - this->_coupl_56 * q[4];
799 
800  double c1 = cos(q[3]);
801  double s1 = sin(q[3]);
802  double c2 = cos(q[4]);
803  double s2 = sin(q[4]);
804  double c3 = cos(q5);
805  double s3 = sin(q5);
806 
807  // Compute the direct geometric model: fMe = transformation betwee
808  // fix and end effector frame.
809  fMe[0][0] = s1*s2*c3 + c1*s3;
810  fMe[0][1] = -s1*s2*s3 + c1*c3;
811  fMe[0][2] = -s1*c2;
812  fMe[0][3] = q0 + this->_long_56*c1;
813 
814  fMe[1][0] = -c1*s2*c3 + s1*s3;
815  fMe[1][1] = c1*s2*s3 + s1*c3;
816  fMe[1][2] = c1*c2;
817  fMe[1][3] = q1 + this->_long_56*s1;
818 
819  fMe[2][0] = c2*c3;
820  fMe[2][1] = -c2*s3;
821  fMe[2][2] = s2;
822  fMe[2][3] = q2;
823 
824  fMe[3][0] = 0;
825  fMe[3][1] = 0;
826  fMe[3][2] = 0;
827  fMe[3][3] = 1;
828 
829  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
830 
831  return;
832 }
833 
844 void
846 {
847  cMe = this->_eMc.inverse();
848 }
849 
859 void
861 {
862  vpHomogeneousMatrix cMe ;
863  get_cMe(cMe) ;
864 
865  cVe.buildFrom(cMe) ;
866 
867  return;
868 }
869 
882 void
884 {
885 
886  eJe.resize(6,6) ;
887 
888  double s3,c3,s4,c4,s5,c5 ;
889 
890  s3=sin(q[3]); c3=cos(q[3]);
891  s4=sin(q[4]); c4=cos(q[4]);
892  s5=sin(q[5]); c5=cos(q[5]);
893 
894  eJe = 0;
895  eJe[0][0] = s3*s4*c5+c3*s5;
896  eJe[0][1] = -c3*s4*c5+s3*s5;
897  eJe[0][2] = c4*c5;
898  eJe[0][3] = - this->_long_56*s4*c5;
899 
900  eJe[1][0] = -s3*s4*s5+c3*c5;
901  eJe[1][1] = c3*s4*s5+s3*c5;
902  eJe[1][2] = -c4*s5;
903  eJe[1][3] = this->_long_56*s4*s5;
904 
905  eJe[2][0] = -s3*c4;
906  eJe[2][1] = c3*c4;
907  eJe[2][2] = s4;
908  eJe[2][3] = this->_long_56*c4;
909 
910  eJe[3][3] = c4*c5;
911  eJe[3][4] = s5;
912 
913  eJe[4][3] = -c4*s5;
914  eJe[4][4] = c5;
915 
916  eJe[5][3] = s4;
917  eJe[5][5] = 1;
918 
919  return;
920 }
921 
922 
949 void
951 {
952 
953  fJe.resize(6,6) ;
954 
955  // block superieur gauche
956  fJe[0][0] = fJe[1][1] = fJe[2][2] = 1 ;
957 
958  double s4 = sin(q[4]) ;
959  double c4 = cos(q[4]) ;
960 
961 
962  // block superieur droit
963  fJe[0][3] = - this->_long_56*s4 ;
964  fJe[1][3] = this->_long_56*c4 ;
965 
966 
967  double s5 = sin(q[5]) ;
968  double c5 = cos(q[5]) ;
969  // block inferieur droit
970  fJe[3][4] = c4 ; fJe[3][5] = -s4*c5 ;
971  fJe[4][4] = s4 ; fJe[4][5] = c4*c5 ;
972  fJe[5][3] = 1 ; fJe[5][5] = s5 ;
973 
974  return;
975 }
976 
977 
988 {
989  vpColVector qmin(6);
990  for (unsigned int i=0; i < 6; i ++)
991  qmin[i] = this->_joint_min[i];
992  return qmin;
993 }
994 
1005 {
1006  vpColVector qmax(6);
1007  for (unsigned int i=0; i < 6; i ++)
1008  qmax[i] = this->_joint_max[i];
1009  return qmax;
1010 }
1011 
1018 double
1020 {
1021  return _coupl_56;
1022 }
1023 
1030 double
1032 {
1033  return _long_56;
1034 }
1035 
1036 
1049 #ifdef VISP_HAVE_ACCESS_TO_NAS
1050 void
1051 vpAfma6::parseConfigFile (const char * filename)
1052 {
1053  size_t dim;
1054  int code;
1055  char Ligne[FILENAME_MAX];
1056  char namoption[100];
1057  FILE * fdtask;
1058  int numLn = 0;
1059  double rot_eMc[3]; // rotation
1060  double trans_eMc[3]; // translation
1061  bool get_rot_eMc = false;
1062  bool get_trans_eMc = false;
1063 
1064  //vpTRACE("Read the config file for constant parameters %s.", filename);
1065  if ((fdtask = fopen(filename, "r" )) == NULL)
1066  {
1067  vpERROR_TRACE ("Impossible to read the config file %s.",
1068  filename);
1070  "Impossible to read the config file.");
1071  }
1072 
1073  while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
1074  numLn ++;
1075  if ('#' == Ligne[0]) { continue; }
1076  sscanf(Ligne, "%s", namoption);
1077  dim = strlen(namoption);
1078 
1079  for (code = 0;
1080  NULL != opt_Afma6[code];
1081  ++ code)
1082  {
1083  if (strncmp(opt_Afma6[code], namoption, dim) == 0)
1084  {
1085  break;
1086  }
1087  }
1088 
1089  switch(code)
1090  {
1091  case 0:
1092  sscanf(Ligne, "%s %lf %lf %lf %lf %lf %lf",
1093  namoption,
1094  &this->_joint_max[0], &this->_joint_max[1],
1095  &this->_joint_max[2], &this->_joint_max[3],
1096  &this->_joint_max[4], &this->_joint_max[5]);
1097  break;
1098 
1099  case 1:
1100  sscanf(Ligne, "%s %lf %lf %lf %lf %lf %lf", namoption,
1101  &this->_joint_min[0], &this->_joint_min[1],
1102  &this->_joint_min[2], &this->_joint_min[3],
1103  &this->_joint_min[4], &this->_joint_min[5]);
1104  break;
1105 
1106  case 2:
1107  sscanf(Ligne, "%s %lf", namoption, &this->_long_56);
1108  break;
1109 
1110  case 3:
1111  sscanf(Ligne, "%s %lf", namoption, &this->_coupl_56);
1112  break;
1113 
1114  case 4:
1115  break; // Nothing to do: camera name
1116 
1117  case 5:
1118  sscanf(Ligne, "%s %lf %lf %lf", namoption,
1119  &rot_eMc[0],
1120  &rot_eMc[1],
1121  &rot_eMc[2]);
1122 
1123  // Convert rotation from degrees to radians
1124  rot_eMc[0] *= M_PI / 180.0;
1125  rot_eMc[1] *= M_PI / 180.0;
1126  rot_eMc[2] *= M_PI / 180.0;
1127  get_rot_eMc = true;
1128  break;
1129 
1130  case 6:
1131  sscanf(Ligne, "%s %lf %lf %lf", namoption,
1132  &trans_eMc[0],
1133  &trans_eMc[1],
1134  &trans_eMc[2]);
1135  get_trans_eMc = true;
1136  break;
1137 
1138  default:
1139  vpERROR_TRACE ("Bad configuration file %s "
1140  "ligne #%d.", filename, numLn);
1141  } /* SWITCH */
1142  } /* WHILE */
1143 
1144  fclose (fdtask);
1145 
1146  // Compute the eMc matrix from the translations and rotations
1147  if (get_rot_eMc && get_trans_eMc) {
1148  for (unsigned int i=0; i < 3; i ++) {
1149  _erc[i] = rot_eMc[i];
1150  _etc[i] = trans_eMc[i];
1151  }
1152 
1153  vpRotationMatrix eRc(_erc);
1154  this->_eMc.buildFrom(_etc, eRc);
1155  }
1156 
1157  return;
1158 }
1159 #endif
1160 
1224 void
1226  const unsigned int &image_width,
1227  const unsigned int &image_height)
1228 {
1229 #if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
1230  vpXmlParserCamera parser;
1231  switch (getToolType()) {
1232  case vpAfma6::TOOL_CCMOP: {
1233  std::cout << "Get camera parameters for camera \""
1234  << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1235  << "from the XML file: \""
1236  << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
1237  if (parser.parse(cam,
1240  projModel,
1241  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1243  "Impossible to read the camera parameters.");
1244  }
1245  break;
1246  }
1247  case vpAfma6::TOOL_GRIPPER: {
1248  std::cout << "Get camera parameters for camera \""
1249  << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1250  << "from the XML file: \""
1251  << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
1252  if (parser.parse(cam,
1255  projModel,
1256  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1258  "Impossible to read the camera parameters.");
1259  }
1260  break;
1261  }
1262  case vpAfma6::TOOL_VACUUM: {
1263  std::cout << "Get camera parameters for camera \""
1264  << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1265  << "from the XML file: \""
1266  << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
1267  if (parser.parse(cam,
1270  projModel,
1271  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1273  "Impossible to read the camera parameters.");
1274  }
1275  break;
1276  }
1278  std::cout << "Get camera parameters for camera \""
1279  << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1280  << "from the XML file: \""
1281  << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
1282  if (parser.parse(cam,
1285  projModel,
1286  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1288  "Impossible to read the camera parameters.");
1289  }
1290  break;
1291  }
1292  default: {
1293  vpERROR_TRACE ("This error should not occur!");
1294  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1295  // "que les specs de la classe ont ete modifiee, "
1296  // "et que le code n'a pas ete mis a jour "
1297  // "correctement.");
1298  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1299  // "vpAfma6::vpAfma6ToolType, et controlez que "
1300  // "tous les cas ont ete pris en compte dans la "
1301  // "fonction init(camera).");
1303  "Impossible to read the camera parameters.");
1304  break;
1305  }
1306  }
1307 #else
1308  // Set default parameters
1309  switch (getToolType()) {
1310  case vpAfma6::TOOL_CCMOP: {
1311  // Set default intrinsic camera parameters for 640x480 images
1312  if (image_width == 640 && image_height == 480) {
1313  std::cout << "Get default camera parameters for camera \""
1314  << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl;
1315  switch(this->projModel) {
1317  cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1318  break;
1320  cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1321  break;
1322  }
1323  }
1324  else {
1325  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
1327  "Impossible to read the camera parameters.");
1328  }
1329  break;
1330  }
1331  case vpAfma6::TOOL_GRIPPER: {
1332  // Set default intrinsic camera parameters for 640x480 images
1333  if (image_width == 640 && image_height == 480) {
1334  std::cout << "Get default camera parameters for camera \""
1335  << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
1336  switch(this->projModel) {
1338  cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
1339  break;
1341  cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
1342  break;
1343  }
1344  }
1345  else {
1346  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
1348  "Impossible to read the camera parameters.");
1349  }
1350  break;
1351  }
1352  case vpAfma6::TOOL_VACUUM: {
1353  // Set default intrinsic camera parameters for 640x480 images
1354  if (image_width == 640 && image_height == 480) {
1355  std::cout << "Get default camera parameters for camera \""
1356  << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl;
1357  switch(this->projModel) {
1359  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1360  break;
1362  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1363  break;
1364  }
1365  }
1366  else {
1367  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
1369  "Impossible to read the camera parameters.");
1370  }
1371  break;
1372  }
1374  // Set default intrinsic camera parameters for 640x480 images
1375  if (image_width == 640 && image_height == 480) {
1376  std::cout << "Get default camera parameters for camera \""
1377  << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
1378  switch(this->projModel) {
1380  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1381  break;
1383  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1384  break;
1385  }
1386  }
1387  else {
1388  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
1390  "Impossible to read the camera parameters.");
1391  }
1392  break;
1393  }
1394  default:
1395  vpERROR_TRACE ("This error should not occur!");
1396  break;
1397  }
1398 #endif
1399  return;
1400 }
1401 
1443 void
1445  const vpImage<unsigned char> &I)
1446 {
1447  getCameraParameters(cam,I.getWidth(),I.getHeight());
1448 }
1492 void
1494  const vpImage<vpRGBa> &I)
1495 {
1496  getCameraParameters(cam,I.getWidth(),I.getHeight());
1497 }
1498 
1499 
1509 std::ostream & operator << (std::ostream & os,
1510  const vpAfma6 & afma6)
1511 {
1512  vpRotationMatrix eRc;
1513  afma6._eMc.extract(eRc);
1514  vpRxyzVector rxyz(eRc);
1515 
1516  os
1517  << "Joint Max:" << std::endl
1518  << "\t" << afma6._joint_max[0]
1519  << "\t" << afma6._joint_max[1]
1520  << "\t" << afma6._joint_max[2]
1521  << "\t" << afma6._joint_max[3]
1522  << "\t" << afma6._joint_max[4]
1523  << "\t" << afma6._joint_max[5]
1524  << "\t" << std::endl
1525 
1526  << "Joint Min: " << std::endl
1527  << "\t" << afma6._joint_min[0]
1528  << "\t" << afma6._joint_min[1]
1529  << "\t" << afma6._joint_min[2]
1530  << "\t" << afma6._joint_min[3]
1531  << "\t" << afma6._joint_min[4]
1532  << "\t" << afma6._joint_min[5]
1533  << "\t" << std::endl
1534 
1535  << "Long 5-6: " << std::endl
1536  << "\t" << afma6._long_56
1537  << "\t" << std::endl
1538 
1539  << "Coupling 5-6:" << std::endl
1540  << "\t" << afma6._coupl_56
1541  << "\t" << std::endl
1542 
1543  << "eMc: "<< std::endl
1544  << "\tTranslation (m): "
1545  << afma6._eMc[0][3] << " "
1546  << afma6._eMc[1][3] << " "
1547  << afma6._eMc[2][3]
1548  << "\t" << std::endl
1549  << "\tRotation Rxyz (rad) : "
1550  << rxyz[0] << " "
1551  << rxyz[1] << " "
1552  << rxyz[2]
1553  << "\t" << std::endl
1554  << "\tRotation Rxyz (deg) : "
1555  << vpMath::deg(rxyz[0]) << " "
1556  << vpMath::deg(rxyz[1]) << " "
1557  << vpMath::deg(rxyz[2])
1558  << "\t" << std::endl;
1559 
1560  return os;
1561 }
1562 /*
1563  * Local variables:
1564  * c-basic-offset: 2
1565  * End:
1566  */