Visual Servoing Platform  version 3.5.0
vpViper850.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 ADEPT Viper 850 robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
47 #include <visp3/core/vpDebug.h>
48 #include <visp3/core/vpMath.h>
49 #include <visp3/core/vpXmlParserCamera.h>
50 #include <visp3/robot/vpViper850.h>
51 
52 static const char *opt_viper850[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL};
53 
54 #ifdef VISP_HAVE_VIPER850_DATA
56  std::string(VISP_VIPER850_DATA_PATH) +
57  std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf");
58 
60  std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf");
61 
63  std::string(VISP_VIPER850_DATA_PATH) +
64  std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf");
65 
67  std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf");
68 
70  std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/"
71  "const_eMc_schunk_gripper_without_distortion_Viper850."
72  "cnf");
73 
75  std::string(VISP_VIPER850_DATA_PATH) +
76  std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf");
77 
79  std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper850.cnf");
80 
82  std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper850.cnf");
83 
84 const std::string vpViper850::CONST_CAMERA_FILENAME =
85  std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml");
86 
87 #endif // VISP_HAVE_VIPER850_DATA
88 
89 const char *const vpViper850::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
90 const char *const vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
91 const char *const vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
92 const char *const vpViper850::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
93 
95 
103  : tool_current(vpViper850::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
104 
105 {
106  // Denavit Hartenberg parameters
107  a1 = 0.075;
108  a2 = 0.365;
109  a3 = 0.090;
110  d1 = 0.335;
111  d4 = 0.405;
112  d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
113  c56 = -341.33 / 9102.22;
114 
115  // Software joint limits in radians
116  joint_min[0] = vpMath::rad(-170);
117  joint_min[1] = vpMath::rad(-190);
118  joint_min[2] = vpMath::rad(-29);
119  joint_min[3] = vpMath::rad(-190);
120  joint_min[4] = vpMath::rad(-120);
121  joint_min[5] = vpMath::rad(-360);
122  joint_max[0] = vpMath::rad(170);
123  joint_max[1] = vpMath::rad(45);
124  joint_max[2] = vpMath::rad(256);
125  joint_max[3] = vpMath::rad(190);
126  joint_max[4] = vpMath::rad(120);
127  joint_max[5] = vpMath::rad(360);
128 
129  init(); // Set the default tool
130 }
131 
137 {
139  return;
140 }
141 
151 void vpViper850::init(const std::string &camera_extrinsic_parameters)
152 {
153  // vpTRACE ("Parse camera file \""%s\"".", camera_filename);
154  this->parseConfigFile(camera_extrinsic_parameters);
155 
156  return;
157 }
158 
181 {
182 
183  this->projModel = proj_model;
184 
185 #ifdef VISP_HAVE_VIPER850_DATA
186  // Read the robot parameters from files
187  std::string filename_eMc;
188  switch (tool) {
190  switch (projModel) {
193  break;
196  break;
198  throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
199  break;
200  }
201  break;
202  }
204  switch (projModel) {
207  break;
210  break;
212  throw vpException(vpException::notImplementedError, "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
213  break;
214  }
215  break;
216  }
218  switch (projModel) {
221  break;
224  break;
226  throw vpException(vpException::notImplementedError, "Feature TOOL_SCHUNK_GRIPPER_CAMERA is not implemented for Kannala-Brandt projection model yet.");
227  break;
228  }
229  break;
230  }
232  switch (projModel) {
235  break;
238  break;
240  throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
241  break;
242  }
243  break;
244  }
247  "No predefined file available for a custom tool"
248  "You should use init(vpViper850::vpToolType, const std::string&) or"
249  "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
250  }
251  default: {
252  vpERROR_TRACE("This error should not occur!");
253  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
254  // "que les specs de la classe ont ete modifiee, "
255  // "et que le code n'a pas ete mis a jour "
256  // "correctement.");
257  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
258  // "vpViper850::vpViper850ToolType, et controlez que "
259  // "tous les cas ont ete pris en compte dans la "
260  // "fonction init(camera).");
261  break;
262  }
263  }
264 
265  this->init(filename_eMc);
266 
267 #else // VISP_HAVE_VIPER850_DATA
268 
269  // Use here default values of the robot constant parameters.
270  switch (tool) {
272  switch (projModel) {
274  erc[0] = vpMath::rad(0.07); // rx
275  erc[1] = vpMath::rad(2.76); // ry
276  erc[2] = vpMath::rad(-91.50); // rz
277  etc[0] = -0.0453; // tx
278  etc[1] = 0.0005; // ty
279  etc[2] = 0.0728; // tz
280  break;
282  erc[0] = vpMath::rad(0.26); // rx
283  erc[1] = vpMath::rad(2.12); // ry
284  erc[2] = vpMath::rad(-91.31); // rz
285  etc[0] = -0.0444; // tx
286  etc[1] = -0.0005; // ty
287  etc[2] = 0.1022; // tz
288  break;
290  throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
291  break;
292  }
293  break;
294  }
297  switch (projModel) {
299  erc[0] = vpMath::rad(0.15); // rx
300  erc[1] = vpMath::rad(1.28); // ry
301  erc[2] = vpMath::rad(-90.8); // rz
302  etc[0] = -0.0456; // tx
303  etc[1] = -0.0013; // ty
304  etc[2] = 0.001; // tz
305  break;
307  erc[0] = vpMath::rad(0.72); // rx
308  erc[1] = vpMath::rad(2.10); // ry
309  erc[2] = vpMath::rad(-90.5); // rz
310  etc[0] = -0.0444; // tx
311  etc[1] = -0.0012; // ty
312  etc[2] = 0.078; // tz
313  break;
315  throw vpException(vpException::notImplementedError, "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
316  break;
317  }
318  break;
319  }
321  // Set eMc to identity
322  switch (projModel) {
325  erc[0] = 0; // rx
326  erc[1] = 0; // ry
327  erc[2] = 0; // rz
328  etc[0] = 0; // tx
329  etc[1] = 0; // ty
330  etc[2] = 0; // tz
331  break;
333  throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
334  break;
335  }
336  break;
337  }
340  "No predefined parameters available for a custom tool"
341  "You should use init(vpViper850::vpToolType, const std::string&) or"
342  "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
343  }
344  }
345  vpRotationMatrix eRc(erc);
346  this->eMc.buildFrom(etc, eRc);
347 #endif // VISP_HAVE_VIPER850_DATA
348 
349  setToolType(tool);
350  return;
351 }
352 
383 void vpViper850::init(vpViper850::vpToolType tool, const std::string &filename)
384 {
385  this->setToolType(tool);
386  this->parseConfigFile(filename.c_str());
387 }
388 
405 {
406  this->setToolType(tool);
407  this->set_eMc(eMc_);
408 }
409 
418 void vpViper850::parseConfigFile(const std::string &filename)
419 {
420  vpRxyzVector erc_; // eMc rotation
421  vpTranslationVector etc_; // eMc translation
422 
423  std::ifstream fdconfig(filename.c_str(), std::ios::in);
424 
425  if (!fdconfig.is_open()) {
426  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
427  filename.c_str());
428  }
429 
430  std::string line;
431  int lineNum = 0;
432  bool get_erc = false;
433  bool get_etc = false;
434  int code;
435 
436  while (std::getline(fdconfig, line)) {
437  lineNum++;
438  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
439  continue;
440  }
441  std::istringstream ss(line);
442  std::string key;
443  ss >> key;
444 
445  for (code = 0; NULL != opt_viper850[code]; ++code) {
446  if (key.compare(opt_viper850[code]) == 0) {
447  break;
448  }
449  }
450 
451  switch (code) {
452  case 0:
453  break; // Nothing to do: camera name
454 
455  case 1: {
456  ss >> erc_[0] >> erc_[1] >> erc_[2];
457 
458  // Convert rotation from degrees to radians
459  erc_ = erc_ * M_PI / 180.0;
460  get_erc = true;
461  break;
462  }
463 
464  case 2: {
465  ss >> etc_[0] >> etc_[1] >> etc_[2];
466  get_etc = true;
467  break;
468  }
469 
470  default:
471  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
472  filename.c_str(), lineNum));
473  }
474  }
475 
476  fdconfig.close();
477 
478  // Compute the eMc matrix from the translations and rotations
479  if (get_etc && get_erc) {
480  this->set_eMc(etc_, erc_);
481  } else {
483  "Could not read translation and rotation "
484  "parameters from config file %s",
485  filename.c_str());
486  }
487 }
488 
558 void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
559  const unsigned int &image_height) const
560 {
561 #if defined(VISP_HAVE_VIPER850_DATA)
562  vpXmlParserCamera parser;
563  switch (getToolType()) {
565  std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
566  << std::endl
567  << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
569  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
570  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
571  }
572  break;
573  }
575  std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
576  << std::endl
577  << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
579  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
580  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
581  }
582  break;
583  }
585  std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
586  << std::endl
587  << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
589  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
590  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
591  }
592  break;
593  }
595  std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
596  << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
598  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
599  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
600  }
601  break;
602  }
604  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
605  }
606  default: {
607  vpERROR_TRACE("This error should not occur!");
608  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
609  // "que les specs de la classe ont ete modifiee, "
610  // "et que le code n'a pas ete mis a jour "
611  // "correctement.");
612  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
613  // "vpViper850::vpViper850ToolType, et controlez que "
614  // "tous les cas ont ete pris en compte dans la "
615  // "fonction init(camera).");
616  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
617  }
618  }
619 #else
620  // Set default parameters
621  switch (getToolType()) {
623  // Set default intrinsic camera parameters for 640x480 images
624  if (image_width == 640 && image_height == 480) {
625  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
626  << std::endl;
627  switch (this->projModel) {
629  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
630  break;
632  cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
633  break;
635  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
636  break;
637  }
638  } else {
639  vpTRACE("Cannot get default intrinsic camera parameters for this image "
640  "resolution");
641  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
642  }
643  break;
644  }
647  // Set default intrinsic camera parameters for 640x480 images
648  if (image_width == 640 && image_height == 480) {
649  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
650  << std::endl;
651  switch (this->projModel) {
653  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
654  break;
656  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
657  break;
659  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
660  break;
661  }
662  } else {
663  vpTRACE("Cannot get default intrinsic camera parameters for this image "
664  "resolution");
665  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
666  }
667  break;
668  }
670  // Set default intrinsic camera parameters for 640x480 images
671  if (image_width == 640 && image_height == 480) {
672  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\""
673  << std::endl;
674  switch (this->projModel) {
676  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
677  break;
679  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
680  break;
682  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
683  break;
684  }
685  } else {
686  vpTRACE("Cannot get default intrinsic camera parameters for this image "
687  "resolution");
688  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
689  }
690  break;
691  }
693  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
694  }
695  default:
696  vpERROR_TRACE("This error should not occur!");
697  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
698  }
699 #endif
700  return;
701 }
702 
764 {
765  getCameraParameters(cam, I.getWidth(), I.getHeight());
766 }
829 {
830  getCameraParameters(cam, I.getWidth(), I.getHeight());
831 }
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ notImplementedError
Not implemented.
Definition: vpException.h:93
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static double rad(double deg)
Definition: vpMath.h:110
Error that can be emited by the vpRobot class and its derivates.
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
Class that consider the case of a translation vector.
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:104
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:123
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:170
static const std::string CONST_CAMERA_FILENAME
Definition: vpViper850.h:117
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:137
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
Definition: vpViper850.h:124
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:116
void parseConfigFile(const std::string &filename)
Definition: vpViper850.cpp:418
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:113
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:114
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpViper850.h:125
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpViper850.cpp:558
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:177
static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:110
static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:111
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:161
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:128
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition: vpViper850.h:131
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper850.h:130
@ TOOL_MARLIN_F033C_CAMERA
Definition: vpViper850.h:129
@ TOOL_GENERIC_CAMERA
Definition: vpViper850.h:132
static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:109
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:122
static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:112
void init(void)
Definition: vpViper850.cpp:136
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:115
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1230
vpTranslationVector etc
Definition: vpViper.h:159
double d6
for joint 6
Definition: vpViper.h:167
double a3
for joint 3
Definition: vpViper.h:165
double d4
for joint 4
Definition: vpViper.h:166
vpColVector joint_max
Definition: vpViper.h:172
double c56
Mechanical coupling between joint 5 and joint 6.
Definition: vpViper.h:169
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:157
double a1
Definition: vpViper.h:163
vpRxyzVector erc
Definition: vpViper.h:160
vpColVector joint_min
Definition: vpViper.h:173
double a2
for joint 2
Definition: vpViper.h:164
double d1
for joint 1
Definition: vpViper.h:163
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0)
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393