ViSP  2.6.2
vpViper850.cpp
1 /****************************************************************************
2  *
3  * $Id: vpViper850.cpp 3842 2012-07-13 22:21:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 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 ADEPT Viper 850 robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
50 #include <visp/vpDebug.h>
51 #include <visp/vpViper850.h>
52 #include <visp/vpMath.h>
53 #include <visp/vpXmlParserCamera.h>
54 
55 #ifdef VISP_HAVE_ACCESS_TO_NAS
56 static const char *opt_viper850[] = {"CAMERA", "eMc_ROT_XYZ","eMc_TRANS_XYZ",
57  NULL};
58 
60 #ifdef WIN32
61 = "Z:/robot/Viper850/current/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf";
62 #else
63 = "/udd/fspindle/robot/Viper850/current/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf";
64 #endif
65 
67 #ifdef WIN32
68 = "Z:/robot/Viper850/current/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf";
69 #else
70 = "/udd/fspindle/robot/Viper850/current/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf";
71 #endif
72 
74 #ifdef WIN32
75 = "Z:/robot/Viper850/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf";
76 #else
77 = "/udd/fspindle/robot/Viper850/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf";
78 #endif
79 
81 #ifdef WIN32
82 = "Z:/robot/Viper850/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf";
83 #else
84 = "/udd/fspindle/robot/Viper850/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf";
85 #endif
86 
88 #ifdef WIN32
89 = "Z:/robot/Viper850/current/include/const_eMc_schunk_gripper_without_distortion_Viper850.cnf";
90 #else
91 = "/udd/fspindle/robot/Viper850/current/include/const_eMc_schunk_gripper_without_distortion_Viper850.cnf";
92 #endif
93 
95 #ifdef WIN32
96 = "Z:/robot/Viper850/current/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf";
97 #else
98 = "/udd/fspindle/robot/Viper850/current/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf";
99 #endif
100 
102 #ifdef WIN32
103 = "Z:/robot/Viper850/current/include/const_eMc_generic_without_distortion_Viper850.cnf";
104 #else
105 = "/udd/fspindle/robot/Viper850/current/include/const_eMc_generic_without_distortion_Viper850.cnf";
106 #endif
107 
109 #ifdef WIN32
110 = "Z:/robot/Viper850/current/include/const_eMc_generic_with_distortion_Viper850.cnf";
111 #else
112 = "/udd/fspindle/robot/Viper850/current/include/const_eMc_generic_with_distortion_Viper850.cnf";
113 #endif
114 
115 
116 const char * const vpViper850::CONST_CAMERA_FILENAME
117 #ifdef WIN32
118 = "Z:/robot/Viper850/current/include/const_camera_Viper850.xml";
119 #else
120 = "/udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml";
121 #endif
122 
123 
124 #endif // VISP_HAVE_ACCESS_TO_NAS
125 
126 const char * const vpViper850::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
127 const char * const vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
128 const char * const vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
129 const char * const vpViper850::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
130 
132 
133 
134 
142 {
143  // Denavit Hartenberg parameters
144  a1 = 0.075;
145  a2 = 0.365;
146  a3 = 0.090;
147  d1 = 0.335;
148  d4 = 0.405;
149  d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
150  c56 = -341.33 / 9102.22;
151 
152  // Software joint limits in radians
153  joint_min[0] = vpMath::rad(-170);
154  joint_min[1] = vpMath::rad(-190);
155  joint_min[2] = vpMath::rad(-29);
156  joint_min[3] = vpMath::rad(-190);
157  joint_min[4] = vpMath::rad(-120);
158  joint_min[5] = vpMath::rad(-360);
159  joint_max[0] = vpMath::rad(170);
160  joint_max[1] = vpMath::rad(45);
161  joint_max[2] = vpMath::rad(256);
162  joint_max[3] = vpMath::rad(190);
163  joint_max[4] = vpMath::rad(120);
164  joint_max[5] = vpMath::rad(360);
165 
166  init(); // Set the default tool
167 }
168 
173 void
175 {
177  return;
178 }
179 
192 #ifdef VISP_HAVE_ACCESS_TO_NAS
193 void
194 vpViper850::init (const char *camera_extrinsic_parameters)
195 {
196  //vpTRACE ("Parse camera file \""%s\"".", camera_filename);
197  this->parseConfigFile (camera_extrinsic_parameters);
198 
199  return ;
200 }
201 #endif
202 
215 void
218 {
219 
220  this->projModel = projModel;
221 
222 #ifdef VISP_HAVE_ACCESS_TO_NAS
223  // Read the robot parameters from files
224  char filename_eMc [FILENAME_MAX];
225  switch (tool) {
227  switch(projModel) {
229 #ifdef UNIX
230  snprintf(filename_eMc, FILENAME_MAX, "%s",
232 #else // WIN32
233  _snprintf(filename_eMc, FILENAME_MAX, "%s",
235 #endif
236  break;
238 #ifdef UNIX
239  snprintf(filename_eMc, FILENAME_MAX, "%s",
241 #else // WIN32
242  _snprintf(filename_eMc, FILENAME_MAX, "%s",
244 #endif
245  break;
246  }
247  break;
248  }
250  switch(projModel) {
252 #ifdef UNIX
253  snprintf(filename_eMc, FILENAME_MAX, "%s",
255 #else // WIN32
256  _snprintf(filename_eMc, FILENAME_MAX, "%s",
258 #endif
259  break;
261 #ifdef UNIX
262  snprintf(filename_eMc, FILENAME_MAX, "%s",
264 #else // WIN32
265  _snprintf(filename_eMc, FILENAME_MAX, "%s",
267 #endif
268  break;
269  }
270  break;
271  }
273  switch(projModel) {
275 #ifdef UNIX
276  snprintf(filename_eMc, FILENAME_MAX, "%s",
278 #else // WIN32
279  _snprintf(filename_eMc, FILENAME_MAX, "%s",
281 #endif
282  break;
284 #ifdef UNIX
285  snprintf(filename_eMc, FILENAME_MAX, "%s",
287 #else // WIN32
288  _snprintf(filename_eMc, FILENAME_MAX, "%s",
290 #endif
291  break;
292  }
293  break;
294  }
296  switch(projModel) {
298 #ifdef UNIX
299  snprintf(filename_eMc, FILENAME_MAX, "%s",
301 #else // WIN32
302  _snprintf(filename_eMc, FILENAME_MAX, "%s",
304 #endif
305  break;
307 #ifdef UNIX
308  snprintf(filename_eMc, FILENAME_MAX, "%s",
310 #else // WIN32
311  _snprintf(filename_eMc, FILENAME_MAX, "%s",
313 #endif
314  break;
315  }
316  break;
317  }
318  default: {
319  vpERROR_TRACE ("This error should not occur!");
320  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
321  // "que les specs de la classe ont ete modifiee, "
322  // "et que le code n'a pas ete mis a jour "
323  // "correctement.");
324  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
325  // "vpViper850::vpViper850ToolType, et controlez que "
326  // "tous les cas ont ete pris en compte dans la "
327  // "fonction init(camera).");
328  break;
329  }
330  }
331 
332  this->init (filename_eMc);
333 
334 #else // VISP_HAVE_ACCESS_TO_NAS
335 
336  // Use here default values of the robot constant parameters.
337  switch (tool) {
339  switch(projModel) {
341  erc[0] = vpMath::rad(0.07); // rx
342  erc[1] = vpMath::rad(2.76); // ry
343  erc[2] = vpMath::rad(-91.50); // rz
344  etc[0] = -0.0453; // tx
345  etc[1] = 0.0005; // ty
346  etc[2] = 0.0728; // tz
347  break;
349  erc[0] = vpMath::rad(0.26); // rx
350  erc[1] = vpMath::rad(2.12); // ry
351  erc[2] = vpMath::rad(-91.31); // rz
352  etc[0] = -0.0444; // tx
353  etc[1] = -0.0005; // ty
354  etc[2] = 0.1022; // tz
355  break;
356  }
357  }
360  switch(projModel) {
362  erc[0] = vpMath::rad(0.15); // rx
363  erc[1] = vpMath::rad(1.28); // ry
364  erc[2] = vpMath::rad(-90.8); // rz
365  etc[0] = -0.0456; // tx
366  etc[1] = -0.0013; // ty
367  etc[2] = 0.001; // tz
368  break;
370  erc[0] = vpMath::rad(0.72); // rx
371  erc[1] = vpMath::rad(2.10); // ry
372  erc[2] = vpMath::rad(-90.5); // rz
373  etc[0] = -0.0444; // tx
374  etc[1] = -0.0012; // ty
375  etc[2] = 0.078; // tz
376  break;
377  }
378  }
380  // Set eMc to identity
381  switch(projModel) {
384  erc[0] = 0; // rx
385  erc[1] = 0; // ry
386  erc[2] = 0; // rz
387  etc[0] = 0; // tx
388  etc[1] = 0; // ty
389  etc[2] = 0; // tz
390  break;
391  }
392  }
393  }
394  vpRotationMatrix eRc(erc);
395  this->eMc.buildFrom(etc, eRc);
396 #endif // VISP_HAVE_ACCESS_TO_NAS
397 
398  setToolType(tool);
399  return ;
400 }
401 
413 #ifdef VISP_HAVE_ACCESS_TO_NAS
414 void
415 vpViper850::parseConfigFile (const char * filename)
416 {
417  size_t dim;
418  int code;
419  char Ligne[FILENAME_MAX];
420  char namoption[100];
421  FILE * fdtask;
422  int numLn = 0;
423  double rot_eMc[3]; // rotation
424  double trans_eMc[3]; // translation
425  bool get_rot_eMc = false;
426  bool get_trans_eMc = false;
427 
428  //vpTRACE("Read the config file for constant parameters %s.", filename);
429  if ((fdtask = fopen(filename, "r" )) == NULL)
430  {
431  vpERROR_TRACE ("Impossible to read the config file %s.",
432  filename);
433  fclose(fdtask);
435  "Impossible to read the config file.");
436  }
437 
438  while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
439  numLn ++;
440  if ('#' == Ligne[0]) { continue; }
441  sscanf(Ligne, "%s", namoption);
442  dim = strlen(namoption);
443 
444  for (code = 0;
445  NULL != opt_viper850[code];
446  ++ code)
447  {
448  if (strncmp(opt_viper850[code], namoption, dim) == 0)
449  {
450  break;
451  }
452  }
453 
454  switch(code) {
455  case 0:
456  break; // Nothing to do: camera name
457 
458  case 1:
459  sscanf(Ligne, "%s %lf %lf %lf", namoption,
460  &rot_eMc[0],
461  &rot_eMc[1],
462  &rot_eMc[2]);
463 
464  // Convert rotation from degrees to radians
465  rot_eMc[0] *= M_PI / 180.0;
466  rot_eMc[1] *= M_PI / 180.0;
467  rot_eMc[2] *= M_PI / 180.0;
468  get_rot_eMc = true;
469  break;
470 
471  case 2:
472  sscanf(Ligne, "%s %lf %lf %lf", namoption,
473  &trans_eMc[0],
474  &trans_eMc[1],
475  &trans_eMc[2]);
476  get_trans_eMc = true;
477  break;
478 
479  default:
480  vpERROR_TRACE ("Bad configuration file %s "
481  "ligne #%d.", filename, numLn);
482  } /* SWITCH */
483  } /* WHILE */
484 
485  fclose (fdtask);
486 
487  // Compute the eMc matrix from the translations and rotations
488  if (get_rot_eMc && get_trans_eMc) {
489  for (unsigned int i=0; i < 3; i ++) {
490  erc[i] = rot_eMc[i];
491  etc[i] = trans_eMc[i];
492  }
493 
494  vpRotationMatrix eRc(erc);
495  this->eMc.buildFrom(etc, eRc);
496  }
497 
498  return;
499 }
500 #endif
501 
502 
575 void
577  const unsigned int &image_width,
578  const unsigned int &image_height)
579 {
580 #if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
581  vpXmlParserCamera parser;
582  switch (getToolType()) {
584  std::cout << "Get camera parameters for camera \""
585  << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl
586  << "from the XML file: \""
587  << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
588  if (parser.parse(cam,
591  projModel,
592  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
594  "Impossible to read the camera parameters.");
595  }
596  break;
597  }
599  std::cout << "Get camera parameters for camera \""
600  << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl
601  << "from the XML file: \""
602  << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
603  if (parser.parse(cam,
606  projModel,
607  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
609  "Impossible to read the camera parameters.");
610  }
611  break;
612  }
614  std::cout << "Get camera parameters for camera \""
615  << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" << std::endl
616  << "from the XML file: \""
617  << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
618  if (parser.parse(cam,
621  projModel,
622  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
624  "Impossible to read the camera parameters.");
625  }
626  break;
627  }
629  std::cout << "Get camera parameters for camera \""
630  << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
631  << "from the XML file: \""
632  << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
633  if (parser.parse(cam,
636  projModel,
637  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
639  "Impossible to read the camera parameters.");
640  }
641  break;
642  }
643  default: {
644  vpERROR_TRACE ("This error should not occur!");
645  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
646  // "que les specs de la classe ont ete modifiee, "
647  // "et que le code n'a pas ete mis a jour "
648  // "correctement.");
649  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
650  // "vpViper850::vpViper850ToolType, et controlez que "
651  // "tous les cas ont ete pris en compte dans la "
652  // "fonction init(camera).");
654  "Impossible to read the camera parameters.");
655  break;
656  }
657  }
658 #else
659  // Set default parameters
660  switch (getToolType()) {
662  // Set default intrinsic camera parameters for 640x480 images
663  if (image_width == 640 && image_height == 480) {
664  std::cout << "Get default camera parameters for camera \""
665  << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
666  switch(this->projModel) {
668  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
669  break;
671  cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
672  break;
673  }
674  }
675  else {
676  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
678  "Impossible to read the camera parameters.");
679  }
680  break;
681  }
684  // Set default intrinsic camera parameters for 640x480 images
685  if (image_width == 640 && image_height == 480) {
686  std::cout << "Get default camera parameters for camera \""
687  << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
688  switch(this->projModel) {
690  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
691  break;
693  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
694  break;
695  }
696  }
697  else {
698  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
700  "Impossible to read the camera parameters.");
701  }
702  break;
703  }
705  // Set default intrinsic camera parameters for 640x480 images
706  if (image_width == 640 && image_height == 480) {
707  std::cout << "Get default camera parameters for camera \""
708  << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
709  switch(this->projModel) {
711  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
712  break;
714  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
715  break;
716  }
717  }
718  else {
719  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
721  "Impossible to read the camera parameters.");
722  }
723  break;
724  }
725  default:
726  vpERROR_TRACE ("This error should not occur!");
728  "Impossible to read the camera parameters.");
729  break;
730  }
731 #endif
732  return;
733 }
734 
798 void
800  const vpImage<unsigned char> &I)
801 {
803 }
868 void
870  const vpImage<vpRGBa> &I)
871 {
873 }
874 
875 
876 
877 /*
878  * Local variables:
879  * c-basic-offset: 2
880  * End:
881  */
vpToolType getToolType()
Get the current tool type.
Definition: vpViper850.h:126
static const char *const CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:75
double a3
for joint 3
Definition: vpViper.h:155
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:147
static const char *const CONST_CAMERA_FILENAME
Definition: vpViper850.h:80
Error that can be emited by the vpRobot class and its derivates.
Perspective projection without distortion model.
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:99
unsigned int getWidth() const
Definition: vpImage.h:154
#define vpERROR_TRACE
Definition: vpDebug.h:379
#define vpTRACE
Definition: vpDebug.h:401
static const char *const CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:73
vpRxyzVector erc
Definition: vpViper.h:150
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:85
XML parser to load and save intrinsic camera parameters.
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
Definition: vpViper850.h:87
double a2
for joint 2
Definition: vpViper.h:154
The vpRotationMatrix considers the particular case of a rotation matrix.
double d1
for joint 1
Definition: vpViper.h:153
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
double c56
Mechanical coupling between joint 5 and joint 6.
Definition: vpViper.h:158
double a1
Definition: vpViper.h:153
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:136
vpTranslationVector etc
Definition: vpViper.h:149
static const char *const CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:72
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpViper850.h:88
vpColVector joint_max
Definition: vpViper.h:161
static const char *const CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:76
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:91
Generic class defining intrinsic camera parameters.
static const char *const CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:77
static const char *const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:79
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
static const char *const CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:74
int parse(vpCameraParameters &cam, const char *filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
Definition: vpViper850.cpp:576
static const char *const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:78
unsigned int getHeight() const
Definition: vpImage.h:145
void parseConfigFile(const char *filename)
Definition: vpViper850.cpp:415
double d4
for joint 4
Definition: vpViper.h:156
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:144
void init(void)
Definition: vpViper850.cpp:174
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_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:86
double d6
for joint 6
Definition: vpViper.h:157
vpColVector joint_min
Definition: vpViper.h:162