Visual Servoing Platform  version 3.5.1 under development (2023-06-08)
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)
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
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:247
unsigned int getHeight() const
Definition: vpImage.h:189
static double rad(double deg)
Definition: vpMath.h:116
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, bool verbose=true)
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393