Visual Servoing Platform  version 3.5.1 under development (2022-08-09)
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;
199  "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
200  break;
201  }
202  break;
203  }
205  switch (projModel) {
208  break;
211  break;
214  "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
215  break;
216  }
217  break;
218  }
220  switch (projModel) {
223  break;
226  break;
228  throw vpException(
230  "Feature TOOL_SCHUNK_GRIPPER_CAMERA is not implemented for Kannala-Brandt projection model yet.");
231  break;
232  }
233  break;
234  }
236  switch (projModel) {
239  break;
242  break;
245  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
246  break;
247  }
248  break;
249  }
252  "No predefined file available for a custom tool"
253  "You should use init(vpViper850::vpToolType, const std::string&) or"
254  "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
255  }
256  default: {
257  vpERROR_TRACE("This error should not occur!");
258  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
259  // "que les specs de la classe ont ete modifiee, "
260  // "et que le code n'a pas ete mis a jour "
261  // "correctement.");
262  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
263  // "vpViper850::vpViper850ToolType, et controlez que "
264  // "tous les cas ont ete pris en compte dans la "
265  // "fonction init(camera).");
266  break;
267  }
268  }
269 
270  this->init(filename_eMc);
271 
272 #else // VISP_HAVE_VIPER850_DATA
273 
274  // Use here default values of the robot constant parameters.
275  switch (tool) {
277  switch (projModel) {
279  erc[0] = vpMath::rad(0.07); // rx
280  erc[1] = vpMath::rad(2.76); // ry
281  erc[2] = vpMath::rad(-91.50); // rz
282  etc[0] = -0.0453; // tx
283  etc[1] = 0.0005; // ty
284  etc[2] = 0.0728; // tz
285  break;
287  erc[0] = vpMath::rad(0.26); // rx
288  erc[1] = vpMath::rad(2.12); // ry
289  erc[2] = vpMath::rad(-91.31); // rz
290  etc[0] = -0.0444; // tx
291  etc[1] = -0.0005; // ty
292  etc[2] = 0.1022; // tz
293  break;
296  "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
297  break;
298  }
299  break;
300  }
303  switch (projModel) {
305  erc[0] = vpMath::rad(0.15); // rx
306  erc[1] = vpMath::rad(1.28); // ry
307  erc[2] = vpMath::rad(-90.8); // rz
308  etc[0] = -0.0456; // tx
309  etc[1] = -0.0013; // ty
310  etc[2] = 0.001; // tz
311  break;
313  erc[0] = vpMath::rad(0.72); // rx
314  erc[1] = vpMath::rad(2.10); // ry
315  erc[2] = vpMath::rad(-90.5); // rz
316  etc[0] = -0.0444; // tx
317  etc[1] = -0.0012; // ty
318  etc[2] = 0.078; // tz
319  break;
322  "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
323  break;
324  }
325  break;
326  }
328  // Set eMc to identity
329  switch (projModel) {
332  erc[0] = 0; // rx
333  erc[1] = 0; // ry
334  erc[2] = 0; // rz
335  etc[0] = 0; // tx
336  etc[1] = 0; // ty
337  etc[2] = 0; // tz
338  break;
341  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
342  break;
343  }
344  break;
345  }
348  "No predefined parameters available for a custom tool"
349  "You should use init(vpViper850::vpToolType, const std::string&) or"
350  "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
351  }
352  }
353  vpRotationMatrix eRc(erc);
354  this->eMc.buildFrom(etc, eRc);
355 #endif // VISP_HAVE_VIPER850_DATA
356 
357  setToolType(tool);
358  return;
359 }
360 
391 void vpViper850::init(vpViper850::vpToolType tool, const std::string &filename)
392 {
393  this->setToolType(tool);
394  this->parseConfigFile(filename.c_str());
395 }
396 
413 {
414  this->setToolType(tool);
415  this->set_eMc(eMc_);
416 }
417 
426 void vpViper850::parseConfigFile(const std::string &filename)
427 {
428  vpRxyzVector erc_; // eMc rotation
429  vpTranslationVector etc_; // eMc translation
430 
431  std::ifstream fdconfig(filename.c_str(), std::ios::in);
432 
433  if (!fdconfig.is_open()) {
434  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
435  filename.c_str());
436  }
437 
438  std::string line;
439  int lineNum = 0;
440  bool get_erc = false;
441  bool get_etc = false;
442  int code;
443 
444  while (std::getline(fdconfig, line)) {
445  lineNum++;
446  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
447  continue;
448  }
449  std::istringstream ss(line);
450  std::string key;
451  ss >> key;
452 
453  for (code = 0; NULL != opt_viper850[code]; ++code) {
454  if (key.compare(opt_viper850[code]) == 0) {
455  break;
456  }
457  }
458 
459  switch (code) {
460  case 0:
461  break; // Nothing to do: camera name
462 
463  case 1: {
464  ss >> erc_[0] >> erc_[1] >> erc_[2];
465 
466  // Convert rotation from degrees to radians
467  erc_ = erc_ * M_PI / 180.0;
468  get_erc = true;
469  break;
470  }
471 
472  case 2: {
473  ss >> etc_[0] >> etc_[1] >> etc_[2];
474  get_etc = true;
475  break;
476  }
477 
478  default:
479  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
480  filename.c_str(), lineNum));
481  }
482  }
483 
484  fdconfig.close();
485 
486  // Compute the eMc matrix from the translations and rotations
487  if (get_etc && get_erc) {
488  this->set_eMc(etc_, erc_);
489  } else {
491  "Could not read translation and rotation "
492  "parameters from config file %s",
493  filename.c_str());
494  }
495 }
496 
566 void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
567  const unsigned int &image_height) const
568 {
569 #if defined(VISP_HAVE_VIPER850_DATA)
570  vpXmlParserCamera parser;
571  switch (getToolType()) {
573  std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
574  << std::endl
575  << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
577  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
578  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
579  }
580  break;
581  }
583  std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
584  << std::endl
585  << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
587  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
588  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
589  }
590  break;
591  }
593  std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
594  << std::endl
595  << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
597  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
598  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
599  }
600  break;
601  }
603  std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
604  << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
606  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
607  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
608  }
609  break;
610  }
612  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
613  }
614  default: {
615  vpERROR_TRACE("This error should not occur!");
616  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
617  // "que les specs de la classe ont ete modifiee, "
618  // "et que le code n'a pas ete mis a jour "
619  // "correctement.");
620  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
621  // "vpViper850::vpViper850ToolType, et controlez que "
622  // "tous les cas ont ete pris en compte dans la "
623  // "fonction init(camera).");
624  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
625  }
626  }
627 #else
628  // Set default parameters
629  switch (getToolType()) {
631  // Set default intrinsic camera parameters for 640x480 images
632  if (image_width == 640 && image_height == 480) {
633  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
634  << std::endl;
635  switch (this->projModel) {
637  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
638  break;
640  cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
641  break;
644  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
645  break;
646  }
647  } else {
648  vpTRACE("Cannot get default intrinsic camera parameters for this image "
649  "resolution");
650  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
651  }
652  break;
653  }
656  // Set default intrinsic camera parameters for 640x480 images
657  if (image_width == 640 && image_height == 480) {
658  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
659  << std::endl;
660  switch (this->projModel) {
662  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
663  break;
665  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
666  break;
669  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
670  break;
671  }
672  } else {
673  vpTRACE("Cannot get default intrinsic camera parameters for this image "
674  "resolution");
675  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
676  }
677  break;
678  }
680  // Set default intrinsic camera parameters for 640x480 images
681  if (image_width == 640 && image_height == 480) {
682  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\""
683  << std::endl;
684  switch (this->projModel) {
686  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
687  break;
689  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
690  break;
693  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
694  break;
695  }
696  } else {
697  vpTRACE("Cannot get default intrinsic camera parameters for this image "
698  "resolution");
699  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
700  }
701  break;
702  }
704  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
705  }
706  default:
707  vpERROR_TRACE("This error should not occur!");
708  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
709  }
710 #endif
711  return;
712 }
713 
775 {
776  getCameraParameters(cam, I.getWidth(), I.getHeight());
777 }
840 {
841  getCameraParameters(cam, I.getWidth(), I.getHeight());
842 }
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 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:426
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:566
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