Visual Servoing Platform  version 3.5.1 under development (2022-08-09)
vpViper650.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 650 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/vpViper650.h>
51 
52 static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL};
53 
54 #ifdef VISP_HAVE_VIPER650_DATA
56  std::string(VISP_VIPER650_DATA_PATH) +
57  std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf");
58 
60  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf");
61 
63  std::string(VISP_VIPER650_DATA_PATH) +
64  std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf");
65 
67  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf");
68 
70  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/"
71  "const_eMc_schunk_gripper_without_distortion_Viper650."
72  "cnf");
73 
75  std::string(VISP_VIPER650_DATA_PATH) +
76  std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf");
77 
79  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper650.cnf");
80 
82  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper650.cnf");
83 
84 const std::string vpViper650::CONST_CAMERA_FILENAME =
85  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_camera_Viper650.xml");
86 
87 #endif // VISP_HAVE_VIPER650_DATA
88 
89 const char *const vpViper650::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
90 const char *const vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
91 const char *const vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
92 const char *const vpViper650::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
93 
95 
103  : tool_current(vpViper650::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
104 {
105  // Denavit Hartenberg parameters
106  a1 = 0.075;
107  a2 = 0.270;
108  a3 = 0.090;
109  d1 = 0.335;
110  d4 = 0.295;
111  d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
112  c56 = -341.33 / 9102.22;
113 
114  // Software joint limits in radians
115  joint_min[0] = vpMath::rad(-170);
116  joint_min[1] = vpMath::rad(-190);
117  joint_min[2] = vpMath::rad(-29);
118  joint_min[3] = vpMath::rad(-190);
119  joint_min[4] = vpMath::rad(-120);
120  joint_min[5] = vpMath::rad(-360);
121  joint_max[0] = vpMath::rad(170);
122  joint_max[1] = vpMath::rad(45);
123  joint_max[2] = vpMath::rad(256);
124  joint_max[3] = vpMath::rad(190);
125  joint_max[4] = vpMath::rad(120);
126  joint_max[5] = vpMath::rad(360);
127 
128  init(); // Set the default tool
129 }
130 
136 {
138  return;
139 }
140 
150 void vpViper650::init(const std::string &camera_extrinsic_parameters)
151 {
152  // vpTRACE ("Parse camera file \""%s\"".", camera_filename);
153  this->parseConfigFile(camera_extrinsic_parameters);
154 
155  return;
156 }
157 
180 {
181 
182  this->projModel = proj_model;
183 
184 #ifdef VISP_HAVE_VIPER650_DATA
185  // Read the robot parameters from files
186  std::string filename_eMc;
187  switch (tool) {
189  switch (projModel) {
192  break;
195  break;
198  "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;
213  "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
214  break;
215  }
216  break;
217  }
219  switch (projModel) {
222  break;
225  break;
227  throw vpException(
229  "Feature TOOL_SCHUNK_GRIPPER_CAMERA is not implemented for Kannala-Brandt projection model yet.");
230  break;
231  }
232  break;
233  }
235  switch (projModel) {
238  break;
241  break;
244  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
245  break;
246  }
247  break;
248  }
251  "No predefined file available for a custom tool"
252  "You should use init(vpViper650::vpToolType, const std::string&) or"
253  "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
254  }
255  default: {
256  vpERROR_TRACE("This error should not occur!");
257  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
258  // "que les specs de la classe ont ete modifiee, "
259  // "et que le code n'a pas ete mis a jour "
260  // "correctement.");
261  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
262  // "vpViper650::vpViper650ToolType, et controlez que "
263  // "tous les cas ont ete pris en compte dans la "
264  // "fonction init(camera).");
265  break;
266  }
267  }
268 
269  this->init(filename_eMc);
270 
271 #else // VISP_HAVE_VIPER650_DATA
272 
273  // Use here default values of the robot constant parameters.
274  switch (tool) {
276  switch (projModel) {
278  erc[0] = vpMath::rad(0.07); // rx
279  erc[1] = vpMath::rad(2.76); // ry
280  erc[2] = vpMath::rad(-91.50); // rz
281  etc[0] = -0.0453; // tx
282  etc[1] = 0.0005; // ty
283  etc[2] = 0.0728; // tz
284  break;
286  erc[0] = vpMath::rad(0.26); // rx
287  erc[1] = vpMath::rad(2.12); // ry
288  erc[2] = vpMath::rad(-91.31); // rz
289  etc[0] = -0.0444; // tx
290  etc[1] = -0.0005; // ty
291  etc[2] = 0.1022; // tz
292  break;
295  "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
296  break;
297  }
298  break;
299  }
302  switch (projModel) {
304  erc[0] = vpMath::rad(0.15); // rx
305  erc[1] = vpMath::rad(1.28); // ry
306  erc[2] = vpMath::rad(-90.8); // rz
307  etc[0] = -0.0456; // tx
308  etc[1] = -0.0013; // ty
309  etc[2] = 0.001; // tz
310  break;
312  erc[0] = vpMath::rad(0.72); // rx
313  erc[1] = vpMath::rad(2.10); // ry
314  erc[2] = vpMath::rad(-90.5); // rz
315  etc[0] = -0.0444; // tx
316  etc[1] = -0.0012; // ty
317  etc[2] = 0.078; // tz
318  break;
321  "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
322  break;
323  }
324  break;
325  }
327  // Set eMc to identity
328  switch (projModel) {
331  erc[0] = 0; // rx
332  erc[1] = 0; // ry
333  erc[2] = 0; // rz
334  etc[0] = 0; // tx
335  etc[1] = 0; // ty
336  etc[2] = 0; // tz
337  break;
340  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
341  break;
342  }
343  break;
344  }
347  "No predefined parameters available for a custom tool"
348  "You should use init(vpViper650::vpToolType, const std::string&) or"
349  "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
350  }
351  }
352  vpRotationMatrix eRc(erc);
353  this->eMc.buildFrom(etc, eRc);
354 #endif // VISP_HAVE_VIPER650_DATA
355 
356  setToolType(tool);
357  return;
358 }
359 
390 void vpViper650::init(vpViper650::vpToolType tool, const std::string &filename)
391 {
392  this->setToolType(tool);
393  this->parseConfigFile(filename.c_str());
394 }
395 
412 {
413  this->setToolType(tool);
414  this->set_eMc(eMc_);
415 }
416 
425 void vpViper650::parseConfigFile(const std::string &filename)
426 {
427  vpRxyzVector erc_; // eMc rotation
428  vpTranslationVector etc_; // eMc translation
429 
430  std::ifstream fdconfig(filename.c_str(), std::ios::in);
431 
432  if (!fdconfig.is_open()) {
433  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
434  filename.c_str());
435  }
436 
437  std::string line;
438  int lineNum = 0;
439  bool get_erc = false;
440  bool get_etc = false;
441  int code;
442 
443  while (std::getline(fdconfig, line)) {
444  lineNum++;
445  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
446  continue;
447  }
448  std::istringstream ss(line);
449  std::string key;
450  ss >> key;
451 
452  for (code = 0; NULL != opt_viper650[code]; ++code) {
453  if (key.compare(opt_viper650[code]) == 0) {
454  break;
455  }
456  }
457 
458  switch (code) {
459  case 0:
460  break; // Nothing to do: camera name
461 
462  case 1: {
463  ss >> erc_[0] >> erc_[1] >> erc_[2];
464 
465  // Convert rotation from degrees to radians
466  erc_ = erc_ * M_PI / 180.0;
467  get_erc = true;
468  break;
469  }
470 
471  case 2: {
472  ss >> etc_[0] >> etc_[1] >> etc_[2];
473  get_etc = true;
474  break;
475  }
476 
477  default:
478  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
479  filename.c_str(), lineNum));
480  }
481  }
482  fdconfig.close();
483 
484  // Compute the eMc matrix from the translations and rotations
485  if (get_etc && get_erc) {
486  this->set_eMc(etc_, erc_);
487  } else {
489  "Could not read translation and rotation "
490  "parameters from config file %s",
491  filename.c_str());
492  }
493 }
494 
564 void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
565  const unsigned int &image_height) const
566 {
567 #if defined(VISP_HAVE_VIPER650_DATA)
568  vpXmlParserCamera parser;
569  switch (getToolType()) {
571  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
572  << std::endl
573  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
575  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
576  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
577  }
578  break;
579  }
581  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
582  << std::endl
583  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
585  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
586  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
587  }
588  break;
589  }
591  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
592  << std::endl
593  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
595  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
596  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
597  }
598  break;
599  }
601  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
602  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
604  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
605  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
606  }
607  break;
608  }
610  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
611  }
612  default: {
613  vpERROR_TRACE("This error should not occur!");
614  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
615  // "que les specs de la classe ont ete modifiee, "
616  // "et que le code n'a pas ete mis a jour "
617  // "correctement.");
618  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
619  // "vpViper650::vpViper650ToolType, et controlez que "
620  // "tous les cas ont ete pris en compte dans la "
621  // "fonction init(camera).");
622  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
623  }
624  }
625 #else
626  // Set default parameters
627  switch (getToolType()) {
629  // Set default intrinsic camera parameters for 640x480 images
630  if (image_width == 640 && image_height == 480) {
631  std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
632  << std::endl;
633  switch (this->projModel) {
635  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
636  break;
638  cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
639  break;
642  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
643  break;
644  }
645  } else {
646  vpTRACE("Cannot get default intrinsic camera parameters for this image "
647  "resolution");
648  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
649  }
650  break;
651  }
654  // Set default intrinsic camera parameters for 640x480 images
655  if (image_width == 640 && image_height == 480) {
656  std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
657  << std::endl;
658  switch (this->projModel) {
660  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
661  break;
663  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
664  break;
667  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
668  break;
669  }
670  } else {
671  vpTRACE("Cannot get default intrinsic camera parameters for this image "
672  "resolution");
673  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
674  }
675  break;
676  }
678  // Set default intrinsic camera parameters for 640x480 images
679  if (image_width == 640 && image_height == 480) {
680  std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\""
681  << std::endl;
682  switch (this->projModel) {
684  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
685  break;
687  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
688  break;
691  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
692  break;
693  }
694  } else {
695  vpTRACE("Cannot get default intrinsic camera parameters for this image "
696  "resolution");
697  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
698  }
699  break;
700  }
702  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
703  }
704  }
705 #endif
706  return;
707 }
708 
770 {
771  getCameraParameters(cam, I.getWidth(), I.getHeight());
772 }
835 {
836  getCameraParameters(cam, I.getWidth(), I.getHeight());
837 }
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:117
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 650 robot.
Definition: vpViper650.h:103
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:114
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper650.h:122
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper650.h:176
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:112
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper650.h:136
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:113
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper650.h:160
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:115
void parseConfigFile(const std::string &filename)
Definition: vpViper650.cpp:425
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpViper650.cpp:564
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpViper650.h:124
void init(void)
Definition: vpViper650.cpp:135
static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:109
void setToolType(vpViper650::vpToolType tool)
Set the current tool type.
Definition: vpViper650.h:169
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper650.h:127
@ TOOL_MARLIN_F033C_CAMERA
Definition: vpViper650.h:128
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition: vpViper650.h:130
@ TOOL_GENERIC_CAMERA
Definition: vpViper650.h:131
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper650.h:129
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper650.h:121
static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:110
static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:111
static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:108
static const std::string CONST_CAMERA_FILENAME
Definition: vpViper650.h:116
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
Definition: vpViper650.h:123
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