ViSP  2.10.0
vpAfma6.cpp
1 /****************************************************************************
2  *
3  * $Id: vpAfma6.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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 #if defined(_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 #if defined(_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 #if defined(_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 #if defined(_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 #if defined(_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 #if defined(_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 #if defined(_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 #if defined(_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 #if defined(_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 #if defined(_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  : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(),
158  tool_current(vpAfma6::defaultTool),
159  projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
160 {
161  // Set the default parameters in case of the config files on the NAS
162  // at Inria are not available.
163 
164  //
165  // Geometric model constant parameters
166  //
167  // coupling between join 5 and 6
168  this->_coupl_56 = 0.009091;
169  // distance between join 5 and 6
170  this->_long_56 = -0.06924;
171  // Camera extrinsic parameters: effector to camera frame
172  this->_eMc.setIdentity(); // Default values are initialized ...
173  // ... in init (vpAfma6::vpAfma6ToolType tool,
174  // vpCameraParameters::vpCameraParametersProjType projModel)
175  // Maximal value of the joints
176  this->_joint_max[0] = 0.7001;
177  this->_joint_max[1] = 0.5201;
178  this->_joint_max[2] = 0.4601;
179  this->_joint_max[3] = 2.7301;
180  this->_joint_max[4] = 2.4801;
181  this->_joint_max[5] = 1.5901;
182  // Minimal value of the joints
183  this->_joint_min[0] = -0.6501;
184  this->_joint_min[1] = -0.6001;
185  this->_joint_min[2] = -0.5001;
186  this->_joint_min[3] = -2.7301;
187  this->_joint_min[4] = -0.1001;
188  this->_joint_min[5] = -1.5901;
189 
190  init();
191 
192 }
193 
198 void
200 {
201  this->init ( vpAfma6::defaultTool);
202  return;
203 }
204 
219 #ifdef VISP_HAVE_ACCESS_TO_NAS
220 void
221 vpAfma6::init (const char * paramAfma6,
222  const char * paramCamera)
223 {
224  // vpTRACE ("Parsage fichier robot.");
225  this->parseConfigFile (paramAfma6);
226 
227  //vpTRACE ("Parsage fichier camera.");
228  this->parseConfigFile (paramCamera);
229 
230  return ;
231 }
232 #endif
233 
246 void
249 {
250 
251  this->projModel = proj_model;
252 
253 #ifdef VISP_HAVE_ACCESS_TO_NAS
254  // Read the robot parameters from files
255  char filename_eMc [FILENAME_MAX];
256  switch (tool) {
257  case vpAfma6::TOOL_CCMOP: {
258  switch(projModel) {
260 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
261  snprintf(filename_eMc, FILENAME_MAX, "%s",
263 #else // _WIN32
264  _snprintf(filename_eMc, FILENAME_MAX, "%s",
266 #endif
267  break;
269 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
270  snprintf(filename_eMc, FILENAME_MAX, "%s",
272 #else // _WIN32
273  _snprintf(filename_eMc, FILENAME_MAX, "%s",
275 #endif
276  break;
277  }
278  break;
279  }
280  case vpAfma6::TOOL_GRIPPER: {
281  switch(projModel) {
283 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
284  snprintf(filename_eMc, FILENAME_MAX, "%s",
286 #else // _WIN32
287  _snprintf(filename_eMc, FILENAME_MAX, "%s",
289 #endif
290  break;
292 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
293  snprintf(filename_eMc, FILENAME_MAX, "%s",
295 #else // _WIN32
296  _snprintf(filename_eMc, FILENAME_MAX, "%s",
298 #endif
299  break;
300  }
301  break;
302  }
303  case vpAfma6::TOOL_VACUUM: {
304  switch(projModel) {
306 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
307  snprintf(filename_eMc, FILENAME_MAX, "%s",
309 #else // _WIN32
310  _snprintf(filename_eMc, FILENAME_MAX, "%s",
312 #endif
313  break;
315 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
316  snprintf(filename_eMc, FILENAME_MAX, "%s",
318 #else // _WIN32
319  _snprintf(filename_eMc, FILENAME_MAX, "%s",
321 #endif
322  break;
323  }
324  break;
325  }
327  switch(projModel) {
329 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
330  snprintf(filename_eMc, FILENAME_MAX, "%s",
332 #else // _WIN32
333  _snprintf(filename_eMc, FILENAME_MAX, "%s",
335 #endif
336  break;
338 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
339  snprintf(filename_eMc, FILENAME_MAX, "%s",
341 #else // _WIN32
342  _snprintf(filename_eMc, FILENAME_MAX, "%s",
344 #endif
345  break;
346  }
347  break;
348  }
349  default: {
350  vpERROR_TRACE ("This error should not occur!");
351  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
352  // "que les specs de la classe ont ete modifiee, "
353  // "et que le code n'a pas ete mis a jour "
354  // "correctement.");
355  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
356  // "vpAfma6::vpAfma6ToolType, et controlez que "
357  // "tous les cas ont ete pris en compte dans la "
358  // "fonction init(camera).");
359  break;
360  }
361  }
362 
363  this->init (vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
364 
365 #else // VISP_HAVE_ACCESS_TO_NAS
366 
367  // Use here default values of the robot constant parameters.
368  switch (tool) {
369  case vpAfma6::TOOL_CCMOP: {
370  switch(projModel) {
372  _erc[0] = vpMath::rad(164.35); // rx
373  _erc[1] = vpMath::rad( 89.64); // ry
374  _erc[2] = vpMath::rad(-73.05); // rz
375  _etc[0] = 0.0117; // tx
376  _etc[1] = 0.0033; // ty
377  _etc[2] = 0.2272; // tz
378  break;
380  _erc[0] = vpMath::rad(33.54); // rx
381  _erc[1] = vpMath::rad(89.34); // ry
382  _erc[2] = vpMath::rad(57.83); // rz
383  _etc[0] = 0.0373; // tx
384  _etc[1] = 0.0024; // ty
385  _etc[2] = 0.2286; // tz
386  break;
387  }
388  break;
389  }
390  case vpAfma6::TOOL_GRIPPER: {
391  switch(projModel) {
393  _erc[0] = vpMath::rad( 88.33); // rx
394  _erc[1] = vpMath::rad( 72.07); // ry
395  _erc[2] = vpMath::rad( 2.53); // rz
396  _etc[0] = 0.0783; // tx
397  _etc[1] = 0.1234; // ty
398  _etc[2] = 0.1638; // tz
399  break;
401  _erc[0] = vpMath::rad(86.69); // rx
402  _erc[1] = vpMath::rad(71.93); // ry
403  _erc[2] = vpMath::rad( 4.17); // rz
404  _etc[0] = 0.1034; // tx
405  _etc[1] = 0.1142; // ty
406  _etc[2] = 0.1642; // tz
407  break;
408  }
409  break;
410  }
411  case vpAfma6::TOOL_VACUUM: {
412  switch(projModel) {
414  _erc[0] = vpMath::rad( 90.40); // rx
415  _erc[1] = vpMath::rad( 75.11); // ry
416  _erc[2] = vpMath::rad( 0.18); // rz
417  _etc[0] = 0.0038; // tx
418  _etc[1] = 0.1281; // ty
419  _etc[2] = 0.1658; // tz
420  break;
422  _erc[0] = vpMath::rad(91.61); // rx
423  _erc[1] = vpMath::rad(76.17); // ry
424  _erc[2] = vpMath::rad(-0.98); // rz
425  _etc[0] = 0.0815; // tx
426  _etc[1] = 0.1162; // ty
427  _etc[2] = 0.1658; // tz
428  break;
429  }
430  break;
431  }
433  switch(projModel) {
436  // set eMc to identity
437  _erc[0] = 0; // rx
438  _erc[1] = 0; // ry
439  _erc[2] = 0; // rz
440  _etc[0] = 0; // tx
441  _etc[1] = 0; // ty
442  _etc[2] = 0; // tz
443  break;
444  }
445  break;
446  }
447  }
448  vpRotationMatrix eRc(_erc);
449  this->_eMc.buildFrom(_etc, eRc);
450 #endif // VISP_HAVE_ACCESS_TO_NAS
451 
452  setToolType(tool);
453  return ;
454 }
455 
456 
483 {
485  fMc = get_fMc(q);
486 
487  return fMc;
488 }
489 
563 int
565  vpColVector & q, const bool &nearest, const bool &verbose) const
566 {
568  double q_[2][6],d[2],t;
569  int ok[2];
570  double cord[6];
571 
572  int nbsol = 0;
573 
574  if (q.getRows() != njoint)
575  q.resize(6);
576 
577 
578  // for(unsigned int i=0;i<3;i++) {
579  // fMe[i][3] = fMc[i][3];
580  // for(int j=0;j<3;j++) {
581  // fMe[i][j] = 0.0;
582  // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
583  // fMe[i][3] -= fMe[i][j]*rpi[j][3];
584  // }
585  // }
586 
587  // std::cout << "\n\nfMc: " << fMc;
588  // std::cout << "\n\neMc: " << _eMc;
589 
590  fMe = fMc * this->_eMc.inverse();
591  // std::cout << "\n\nfMe: " << fMe;
592 
593  if (fMe[2][2] >= .99999f)
594  {
595  vpTRACE("singularity\n");
596  q_[0][4] = q_[1][4] = M_PI/2.f;
597  t = atan2(fMe[0][0],fMe[0][1]);
598  q_[1][3] = q_[0][3] = q[3];
599  q_[1][5] = q_[0][5] = t - q_[0][3];
600 
601  while ((q_[1][5]+vpMath::rad(2)) >= this->_joint_max[5])
602  /* -> a cause du couplage 4/5 */
603  {
604  q_[1][5] -= vpMath::rad(10);
605  q_[1][3] += vpMath::rad(10);
606  }
607  while (q_[1][5] <= this->_joint_min[5])
608  {
609  q_[1][5] += vpMath::rad(10);
610  q_[1][3] -= vpMath::rad(10);
611  }
612  }
613  else if (fMe[2][2] <= -.99999)
614  {
615  vpTRACE("singularity\n");
616  q_[0][4] = q_[1][4] = -M_PI/2;
617  t = atan2(fMe[1][1],fMe[1][0]);
618  q_[1][3] = q_[0][3] = q[3];
619  q_[1][5] = q_[0][5] = q_[0][3] - t;
620  while ((q_[1][5]+vpMath::rad(2)) >= this->_joint_max[5])
621  /* -> a cause du couplage 4/5 */
622  {
623  q_[1][5] -= vpMath::rad(10);
624  q_[1][3] -= vpMath::rad(10);
625  }
626  while (q_[1][5] <= this->_joint_min[5])
627  {
628  q_[1][5] += vpMath::rad(10);
629  q_[1][3] += vpMath::rad(10);
630  }
631  }
632  else
633  {
634  q_[0][3] = atan2(-fMe[0][2],fMe[1][2]);
635  if (q_[0][3] >= 0.0) q_[1][3] = q_[0][3] - M_PI;
636  else q_[1][3] = q_[0][3] + M_PI;
637 
638  q_[0][4] = asin(fMe[2][2]);
639  if (q_[0][4] >= 0.0) q_[1][4] = M_PI - q_[0][4];
640  else q_[1][4] = -M_PI - q_[0][4];
641 
642  q_[0][5] = atan2(-fMe[2][1],fMe[2][0]);
643  if (q_[0][5] >= 0.0) q_[1][5] = q_[0][5] - M_PI;
644  else q_[1][5] = q_[0][5] + M_PI;
645  }
646  q_[0][0] = fMe[0][3] - this->_long_56*cos(q_[0][3]);
647  q_[1][0] = fMe[0][3] - this->_long_56*cos(q_[1][3]);
648  q_[0][1] = fMe[1][3] - this->_long_56*sin(q_[0][3]);
649  q_[1][1] = fMe[1][3] - this->_long_56*sin(q_[1][3]);
650  q_[0][2] = q_[1][2] = fMe[2][3];
651 
652  /* prise en compte du couplage axes 5/6 */
653  q_[0][5] += this->_coupl_56*q_[0][4];
654  q_[1][5] += this->_coupl_56*q_[1][4];
655 
656  for (int j=0;j<2;j++)
657  {
658  ok[j] = 1;
659  // test is position is reachable
660  for (unsigned int i=0;i<6;i++) {
661  if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
662  if (verbose) {
663  if (i < 3)
664  std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < " << this->_joint_max[i] << std::endl;
665  else
666  std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < " << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
667  }
668  ok[j] = 0;
669  }
670  }
671  }
672  if (ok[0] == 0)
673  {
674  if (ok[1] == 0) {
675  std::cout << "No solution..." << std::endl;
676  nbsol = 0;
677  return nbsol;
678  }
679  else if (ok[1] == 1) {
680  for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
681  nbsol = 1;
682  }
683  }
684  else
685  {
686  if (ok[1] == 0) {
687  for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
688  nbsol = 1;
689  }
690  else
691  {
692  nbsol = 2;
693  //vpTRACE("2 solutions\n");
694  for (int j=0;j<2;j++)
695  {
696  d[j] = 0.0;
697  for (unsigned int i=3;i<6;i++)
698  d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
699  }
700  if (nearest == true)
701  {
702  if (d[0] <= d[1])
703  for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
704  else
705  for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
706  }
707  else
708  {
709  if (d[0] <= d[1])
710  for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
711  else
712  for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
713  }
714  }
715  }
716  for(unsigned int i=0; i<6; i++)
717  q[i] = cord[i] ;
718 
719  return nbsol;
720 }
721 
745 vpAfma6::get_fMc (const vpColVector & q) const
746 {
748  get_fMc(q, fMc);
749 
750  return fMc;
751 }
752 
772 void
774 {
775 
776  // Compute the direct geometric model: fMe = transformation between
777  // fix and end effector frame.
779 
780  get_fMe(q, fMe);
781 
782  fMc = fMe * this->_eMc;
783 
784  return;
785 }
786 
806 void
808 {
809  double q0 = q[0]; // meter
810  double q1 = q[1]; // meter
811  double q2 = q[2]; // meter
812 
813  /* Decouplage liaisons 2 et 3. */
814  double q5 = q[5] - this->_coupl_56 * q[4];
815 
816  double c1 = cos(q[3]);
817  double s1 = sin(q[3]);
818  double c2 = cos(q[4]);
819  double s2 = sin(q[4]);
820  double c3 = cos(q5);
821  double s3 = sin(q5);
822 
823  // Compute the direct geometric model: fMe = transformation betwee
824  // fix and end effector frame.
825  fMe[0][0] = s1*s2*c3 + c1*s3;
826  fMe[0][1] = -s1*s2*s3 + c1*c3;
827  fMe[0][2] = -s1*c2;
828  fMe[0][3] = q0 + this->_long_56*c1;
829 
830  fMe[1][0] = -c1*s2*c3 + s1*s3;
831  fMe[1][1] = c1*s2*s3 + s1*c3;
832  fMe[1][2] = c1*c2;
833  fMe[1][3] = q1 + this->_long_56*s1;
834 
835  fMe[2][0] = c2*c3;
836  fMe[2][1] = -c2*s3;
837  fMe[2][2] = s2;
838  fMe[2][3] = q2;
839 
840  fMe[3][0] = 0;
841  fMe[3][1] = 0;
842  fMe[3][2] = 0;
843  fMe[3][3] = 1;
844 
845  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
846 
847  return;
848 }
849 
860 void
862 {
863  cMe = this->_eMc.inverse();
864 }
865 
875 void
877 {
878  vpHomogeneousMatrix cMe ;
879  get_cMe(cMe) ;
880 
881  cVe.buildFrom(cMe) ;
882 
883  return;
884 }
885 
898 void
899 vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
900 {
901 
902  eJe.resize(6,6) ;
903 
904  double s4,c4,s5,c5,s6,c6 ;
905 
906  s4=sin(q[3]); c4=cos(q[3]);
907  s5=sin(q[4]); c5=cos(q[4]);
908  s6=sin(q[5]-this->_coupl_56*q[4]); c6=cos(q[5]-this->_coupl_56*q[4]);
909 
910  eJe = 0;
911  eJe[0][0] = s4*s5*c6+c4*s6;
912  eJe[0][1] = -c4*s5*c6+s4*s6;
913  eJe[0][2] = c5*c6;
914  eJe[0][3] = - this->_long_56*s5*c6;
915 
916  eJe[1][0] = -s4*s5*s6+c4*c6;
917  eJe[1][1] = c4*s5*s6+s4*c6;
918  eJe[1][2] = -c5*s6;
919  eJe[1][3] = this->_long_56*s5*s6;
920 
921  eJe[2][0] = -s4*c5;
922  eJe[2][1] = c4*c5;
923  eJe[2][2] = s5;
924  eJe[2][3] = this->_long_56*c5;
925 
926  eJe[3][3] = c5*c6;
927  eJe[3][4] = s6;
928 
929  eJe[4][3] = -c5*s6;
930  eJe[4][4] = c6;
931 
932  eJe[5][3] = s5;
933  eJe[5][4] = -this->_coupl_56;
934  eJe[5][5] = 1;
935 
936  return;
937 }
938 
939 
967 void
968 vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
969 {
970 
971  fJe.resize(6,6) ;
972 
973  // block superieur gauche
974  fJe[0][0] = fJe[1][1] = fJe[2][2] = 1 ;
975 
976  double s4 = sin(q[3]) ;
977  double c4 = cos(q[3]) ;
978 
979 
980  // block superieur droit
981  fJe[0][3] = - this->_long_56*s4 ;
982  fJe[1][3] = this->_long_56*c4 ;
983 
984 
985  double s5 = sin(q[4]) ;
986  double c5 = cos(q[4]) ;
987  // block inferieur droit
988  fJe[3][4] = c4 ; fJe[3][5] = -s4*c5 ;
989  fJe[4][4] = s4 ; fJe[4][5] = c4*c5 ;
990  fJe[5][3] = 1 ; fJe[5][5] = s5 ;
991 
992  // coupling between joint 5 and 6
993  fJe[3][4] += this->_coupl_56*s4*c5;
994  fJe[4][4] += -this->_coupl_56*c4*c5;
995  fJe[5][4] += -this->_coupl_56*s5;
996 
997  return;
998 }
999 
1000 
1011 {
1012  vpColVector qmin(6);
1013  for (unsigned int i=0; i < 6; i ++)
1014  qmin[i] = this->_joint_min[i];
1015  return qmin;
1016 }
1017 
1028 {
1029  vpColVector qmax(6);
1030  for (unsigned int i=0; i < 6; i ++)
1031  qmax[i] = this->_joint_max[i];
1032  return qmax;
1033 }
1034 
1041 double
1043 {
1044  return _coupl_56;
1045 }
1046 
1053 double
1055 {
1056  return _long_56;
1057 }
1058 
1059 
1072 #ifdef VISP_HAVE_ACCESS_TO_NAS
1073 void
1074 vpAfma6::parseConfigFile (const char * filename)
1075 {
1076  size_t dim;
1077  int code;
1078  char Ligne[FILENAME_MAX];
1079  char namoption[100];
1080  FILE * fdtask;
1081  int numLn = 0;
1082  double rot_eMc[3]; // rotation
1083  double trans_eMc[3]; // translation
1084  bool get_rot_eMc = false;
1085  bool get_trans_eMc = false;
1086 
1087  //vpTRACE("Read the config file for constant parameters %s.", filename);
1088  if ((fdtask = fopen(filename, "r" )) == NULL)
1089  {
1090  vpERROR_TRACE ("Impossible to read the config file %s.",
1091  filename);
1093  "Impossible to read the config file.");
1094  }
1095 
1096  while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
1097  numLn ++;
1098  if ('#' == Ligne[0]) { continue; }
1099  sscanf(Ligne, "%s", namoption);
1100  dim = strlen(namoption);
1101 
1102  for (code = 0;
1103  NULL != opt_Afma6[code];
1104  ++ code)
1105  {
1106  if (strncmp(opt_Afma6[code], namoption, dim) == 0)
1107  {
1108  break;
1109  }
1110  }
1111 
1112  switch(code)
1113  {
1114  case 0:
1115  sscanf(Ligne, "%s %lf %lf %lf %lf %lf %lf",
1116  namoption,
1117  &this->_joint_max[0], &this->_joint_max[1],
1118  &this->_joint_max[2], &this->_joint_max[3],
1119  &this->_joint_max[4], &this->_joint_max[5]);
1120  break;
1121 
1122  case 1:
1123  sscanf(Ligne, "%s %lf %lf %lf %lf %lf %lf", namoption,
1124  &this->_joint_min[0], &this->_joint_min[1],
1125  &this->_joint_min[2], &this->_joint_min[3],
1126  &this->_joint_min[4], &this->_joint_min[5]);
1127  break;
1128 
1129  case 2:
1130  sscanf(Ligne, "%s %lf", namoption, &this->_long_56);
1131  break;
1132 
1133  case 3:
1134  sscanf(Ligne, "%s %lf", namoption, &this->_coupl_56);
1135  break;
1136 
1137  case 4:
1138  break; // Nothing to do: camera name
1139 
1140  case 5:
1141  sscanf(Ligne, "%s %lf %lf %lf", namoption,
1142  &rot_eMc[0],
1143  &rot_eMc[1],
1144  &rot_eMc[2]);
1145 
1146  // Convert rotation from degrees to radians
1147  rot_eMc[0] *= M_PI / 180.0;
1148  rot_eMc[1] *= M_PI / 180.0;
1149  rot_eMc[2] *= M_PI / 180.0;
1150  get_rot_eMc = true;
1151  break;
1152 
1153  case 6:
1154  sscanf(Ligne, "%s %lf %lf %lf", namoption,
1155  &trans_eMc[0],
1156  &trans_eMc[1],
1157  &trans_eMc[2]);
1158  get_trans_eMc = true;
1159  break;
1160 
1161  default:
1162  vpERROR_TRACE ("Bad configuration file %s "
1163  "ligne #%d.", filename, numLn);
1164  } /* SWITCH */
1165  } /* WHILE */
1166 
1167  fclose (fdtask);
1168 
1169  // Compute the eMc matrix from the translations and rotations
1170  if (get_rot_eMc && get_trans_eMc) {
1171  for (unsigned int i=0; i < 3; i ++) {
1172  _erc[i] = rot_eMc[i];
1173  _etc[i] = trans_eMc[i];
1174  }
1175 
1176  vpRotationMatrix eRc(_erc);
1177  this->_eMc.buildFrom(_etc, eRc);
1178  }
1179 
1180  return;
1181 }
1182 #endif
1183 
1247 void
1249  const unsigned int &image_width,
1250  const unsigned int &image_height) const
1251 {
1252 #if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
1253  vpXmlParserCamera parser;
1254  switch (getToolType()) {
1255  case vpAfma6::TOOL_CCMOP: {
1256  std::cout << "Get camera parameters for camera \""
1257  << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1258  << "from the XML file: \""
1259  << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
1260  if (parser.parse(cam,
1263  projModel,
1264  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1266  "Impossible to read the camera parameters.");
1267  }
1268  break;
1269  }
1270  case vpAfma6::TOOL_GRIPPER: {
1271  std::cout << "Get camera parameters for camera \""
1272  << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1273  << "from the XML file: \""
1274  << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
1275  if (parser.parse(cam,
1278  projModel,
1279  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1281  "Impossible to read the camera parameters.");
1282  }
1283  break;
1284  }
1285  case vpAfma6::TOOL_VACUUM: {
1286  std::cout << "Get camera parameters for camera \""
1287  << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1288  << "from the XML file: \""
1289  << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
1290  if (parser.parse(cam,
1293  projModel,
1294  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1296  "Impossible to read the camera parameters.");
1297  }
1298  break;
1299  }
1301  std::cout << "Get camera parameters for camera \""
1302  << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1303  << "from the XML file: \""
1304  << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
1305  if (parser.parse(cam,
1308  projModel,
1309  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1311  "Impossible to read the camera parameters.");
1312  }
1313  break;
1314  }
1315  default: {
1316  vpERROR_TRACE ("This error should not occur!");
1317  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1318  // "que les specs de la classe ont ete modifiee, "
1319  // "et que le code n'a pas ete mis a jour "
1320  // "correctement.");
1321  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1322  // "vpAfma6::vpAfma6ToolType, et controlez que "
1323  // "tous les cas ont ete pris en compte dans la "
1324  // "fonction init(camera).");
1326  "Impossible to read the camera parameters.");
1327  break;
1328  }
1329  }
1330 #else
1331  // Set default parameters
1332  switch (getToolType()) {
1333  case vpAfma6::TOOL_CCMOP: {
1334  // Set default intrinsic camera parameters for 640x480 images
1335  if (image_width == 640 && image_height == 480) {
1336  std::cout << "Get default camera parameters for camera \""
1337  << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl;
1338  switch(this->projModel) {
1340  cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1341  break;
1343  cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1344  break;
1345  }
1346  }
1347  else {
1348  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
1350  "Impossible to read the camera parameters.");
1351  }
1352  break;
1353  }
1354  case vpAfma6::TOOL_GRIPPER: {
1355  // Set default intrinsic camera parameters for 640x480 images
1356  if (image_width == 640 && image_height == 480) {
1357  std::cout << "Get default camera parameters for camera \""
1358  << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
1359  switch(this->projModel) {
1361  cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
1362  break;
1364  cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
1365  break;
1366  }
1367  }
1368  else {
1369  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
1371  "Impossible to read the camera parameters.");
1372  }
1373  break;
1374  }
1375  case vpAfma6::TOOL_VACUUM: {
1376  // Set default intrinsic camera parameters for 640x480 images
1377  if (image_width == 640 && image_height == 480) {
1378  std::cout << "Get default camera parameters for camera \""
1379  << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl;
1380  switch(this->projModel) {
1382  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1383  break;
1385  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1386  break;
1387  }
1388  }
1389  else {
1390  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
1392  "Impossible to read the camera parameters.");
1393  }
1394  break;
1395  }
1397  // Set default intrinsic camera parameters for 640x480 images
1398  if (image_width == 640 && image_height == 480) {
1399  std::cout << "Get default camera parameters for camera \""
1400  << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
1401  switch(this->projModel) {
1403  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1404  break;
1406  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1407  break;
1408  }
1409  }
1410  else {
1411  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
1413  "Impossible to read the camera parameters.");
1414  }
1415  break;
1416  }
1417  default:
1418  vpERROR_TRACE ("This error should not occur!");
1419  break;
1420  }
1421 #endif
1422  return;
1423 }
1424 
1466 void
1468  const vpImage<unsigned char> &I) const
1469 {
1470  getCameraParameters(cam,I.getWidth(),I.getHeight());
1471 }
1515 void
1517  const vpImage<vpRGBa> &I) const
1518 {
1519  getCameraParameters(cam,I.getWidth(),I.getHeight());
1520 }
1521 
1522 
1532 VISP_EXPORT std::ostream & operator << (std::ostream & os,
1533  const vpAfma6 & afma6)
1534 {
1535  vpRotationMatrix eRc;
1536  afma6._eMc.extract(eRc);
1537  vpRxyzVector rxyz(eRc);
1538 
1539  os
1540  << "Joint Max:" << std::endl
1541  << "\t" << afma6._joint_max[0]
1542  << "\t" << afma6._joint_max[1]
1543  << "\t" << afma6._joint_max[2]
1544  << "\t" << afma6._joint_max[3]
1545  << "\t" << afma6._joint_max[4]
1546  << "\t" << afma6._joint_max[5]
1547  << "\t" << std::endl
1548 
1549  << "Joint Min: " << std::endl
1550  << "\t" << afma6._joint_min[0]
1551  << "\t" << afma6._joint_min[1]
1552  << "\t" << afma6._joint_min[2]
1553  << "\t" << afma6._joint_min[3]
1554  << "\t" << afma6._joint_min[4]
1555  << "\t" << afma6._joint_min[5]
1556  << "\t" << std::endl
1557 
1558  << "Long 5-6: " << std::endl
1559  << "\t" << afma6._long_56
1560  << "\t" << std::endl
1561 
1562  << "Coupling 5-6:" << std::endl
1563  << "\t" << afma6._coupl_56
1564  << "\t" << std::endl
1565 
1566  << "eMc: "<< std::endl
1567  << "\tTranslation (m): "
1568  << afma6._eMc[0][3] << " "
1569  << afma6._eMc[1][3] << " "
1570  << afma6._eMc[2][3]
1571  << "\t" << std::endl
1572  << "\tRotation Rxyz (rad) : "
1573  << rxyz[0] << " "
1574  << rxyz[1] << " "
1575  << rxyz[2]
1576  << "\t" << std::endl
1577  << "\tRotation Rxyz (deg) : "
1578  << vpMath::deg(rxyz[0]) << " "
1579  << vpMath::deg(rxyz[1]) << " "
1580  << vpMath::deg(rxyz[2])
1581  << "\t" << std::endl;
1582 
1583  return os;
1584 }
1585 
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:69
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
vpRxyzVector _erc
Definition: vpAfma6.h:190
vpTranslationVector _etc
Definition: vpAfma6.h:189
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1248
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:176
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:876
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
Error that can be emited by the vpRobot class and its derivates.
static const char *const CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:79
Perspective projection without distortion model.
static const char *const CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:78
unsigned int getWidth() const
Definition: vpImage.h:161
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma6.cpp:482
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:564
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:150
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
#define vpTRACE
Definition: vpDebug.h:418
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:745
void setIdentity()
Basic initialisation (identity)
double _coupl_56
Definition: vpAfma6.h:184
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:899
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:108
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:198
static const char *const CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:82
XML parser to load and save intrinsic camera parameters.
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma6.cpp:807
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:174
static const char *const CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:81
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:95
The vpRotationMatrix considers the particular case of a rotation matrix.
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:117
static const char *const CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:77
static const char *const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:83
vpColVector getJointMin() const
Definition: vpAfma6.cpp:1010
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static const char *const CONST_CAMERA_AFMA6_FILENAME
Definition: vpAfma6.h:85
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:861
static const char *const CONST_AFMA6_FILENAME
Definition: vpAfma6.h:76
Generic class defining intrinsic camera parameters.
void parseConfigFile(const char *filename)
Definition: vpAfma6.cpp:1074
vpColVector getJointMax() const
Definition: vpAfma6.cpp:1027
void extract(vpRotationMatrix &R) const
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
vpAfma6()
Definition: vpAfma6.cpp:156
Perspective projection with distortion model.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static double rad(double deg)
Definition: vpMath.h:100
double getLong56() const
Definition: vpAfma6.cpp:1054
double _long_56
Definition: vpAfma6.h:185
static double deg(double rad)
Definition: vpMath.h:93
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
static const char *const CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:80
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:192
unsigned int getHeight() const
Definition: vpImage.h:152
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
void init(void)
Definition: vpAfma6.cpp:199
double _joint_max[6]
Definition: vpAfma6.h:186
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)
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:90
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:968
static const char *const CONST_VACUUM_CAMERA_NAME
Definition: vpAfma6.h:100
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
static const char *const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:84
double _joint_min[6]
Definition: vpAfma6.h:187
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpAfma6.h:105
double getCoupl56() const
Definition: vpAfma6.cpp:1042