Visual Servoing Platform  version 3.6.1 under development (2023-11-30)
vpViper650.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
44 #include <visp3/core/vpDebug.h>
45 #include <visp3/core/vpMath.h>
46 #include <visp3/core/vpXmlParserCamera.h>
47 #include <visp3/robot/vpViper650.h>
48 
49 static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr};
50 
51 #ifdef VISP_HAVE_VIPER650_DATA
53  std::string(VISP_VIPER650_DATA_PATH) +
54  std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf");
55 
57  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf");
58 
60  std::string(VISP_VIPER650_DATA_PATH) +
61  std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf");
62 
64  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf");
65 
67  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/"
68  "const_eMc_schunk_gripper_without_distortion_Viper650."
69  "cnf");
70 
72  std::string(VISP_VIPER650_DATA_PATH) +
73  std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf");
74 
76  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper650.cnf");
77 
79  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper650.cnf");
80 
81 const std::string vpViper650::CONST_CAMERA_FILENAME =
82  std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_camera_Viper650.xml");
83 
84 #endif // VISP_HAVE_VIPER650_DATA
85 
86 const char *const vpViper650::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
87 const char *const vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
88 const char *const vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
89 const char *const vpViper650::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
90 
92 
100  : tool_current(vpViper650::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
101 {
102  // Denavit-Hartenberg parameters
103  a1 = 0.075;
104  a2 = 0.270;
105  a3 = 0.090;
106  d1 = 0.335;
107  d4 = 0.295;
108  d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
109  c56 = -341.33 / 9102.22;
110 
111  // Software joint limits in radians
112  joint_min[0] = vpMath::rad(-170);
113  joint_min[1] = vpMath::rad(-190);
114  joint_min[2] = vpMath::rad(-29);
115  joint_min[3] = vpMath::rad(-190);
116  joint_min[4] = vpMath::rad(-120);
117  joint_min[5] = vpMath::rad(-360);
118  joint_max[0] = vpMath::rad(170);
119  joint_max[1] = vpMath::rad(45);
120  joint_max[2] = vpMath::rad(256);
121  joint_max[3] = vpMath::rad(190);
122  joint_max[4] = vpMath::rad(120);
123  joint_max[5] = vpMath::rad(360);
124 
125  init(); // Set the default tool
126 }
127 
133 {
135  return;
136 }
137 
147 void vpViper650::init(const std::string &camera_extrinsic_parameters)
148 {
149  // vpTRACE ("Parse camera file \""%s\"".", camera_filename);
150  this->parseConfigFile(camera_extrinsic_parameters);
151 
152  return;
153 }
154 
177 {
178 
179  this->projModel = proj_model;
180 
181 #ifdef VISP_HAVE_VIPER650_DATA
182  // Read the robot parameters from files
183  std::string filename_eMc;
184  switch (tool) {
186  switch (projModel) {
189  break;
192  break;
195  "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
196  break;
197  }
198  break;
199  }
201  switch (projModel) {
204  break;
207  break;
210  "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
211  break;
212  }
213  break;
214  }
216  switch (projModel) {
219  break;
222  break;
224  throw vpException(
226  "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;
241  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
242  break;
243  }
244  break;
245  }
248  "No predefined file available for a custom tool"
249  "You should use init(vpViper650::vpToolType, const std::string&) or"
250  "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
251  }
252  default: {
253  vpERROR_TRACE("This error should not occur!");
254  break;
255  }
256  }
257 
258  this->init(filename_eMc);
259 
260 #else // VISP_HAVE_VIPER650_DATA
261 
262  // Use here default values of the robot constant parameters.
263  switch (tool) {
265  switch (projModel) {
267  erc[0] = vpMath::rad(0.07); // rx
268  erc[1] = vpMath::rad(2.76); // ry
269  erc[2] = vpMath::rad(-91.50); // rz
270  etc[0] = -0.0453; // tx
271  etc[1] = 0.0005; // ty
272  etc[2] = 0.0728; // tz
273  break;
275  erc[0] = vpMath::rad(0.26); // rx
276  erc[1] = vpMath::rad(2.12); // ry
277  erc[2] = vpMath::rad(-91.31); // rz
278  etc[0] = -0.0444; // tx
279  etc[1] = -0.0005; // ty
280  etc[2] = 0.1022; // tz
281  break;
284  "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
285  break;
286  }
287  break;
288  }
291  switch (projModel) {
293  erc[0] = vpMath::rad(0.15); // rx
294  erc[1] = vpMath::rad(1.28); // ry
295  erc[2] = vpMath::rad(-90.8); // rz
296  etc[0] = -0.0456; // tx
297  etc[1] = -0.0013; // ty
298  etc[2] = 0.001; // tz
299  break;
301  erc[0] = vpMath::rad(0.72); // rx
302  erc[1] = vpMath::rad(2.10); // ry
303  erc[2] = vpMath::rad(-90.5); // rz
304  etc[0] = -0.0444; // tx
305  etc[1] = -0.0012; // ty
306  etc[2] = 0.078; // tz
307  break;
310  "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
311  break;
312  }
313  break;
314  }
316  // Set eMc to identity
317  switch (projModel) {
320  erc[0] = 0; // rx
321  erc[1] = 0; // ry
322  erc[2] = 0; // rz
323  etc[0] = 0; // tx
324  etc[1] = 0; // ty
325  etc[2] = 0; // tz
326  break;
329  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
330  break;
331  }
332  break;
333  }
336  "No predefined parameters available for a custom tool"
337  "You should use init(vpViper650::vpToolType, const std::string&) or"
338  "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
339  }
340  }
341  vpRotationMatrix eRc(erc);
342  this->eMc.buildFrom(etc, eRc);
343 #endif // VISP_HAVE_VIPER650_DATA
344 
345  setToolType(tool);
346  return;
347 }
348 
379 void vpViper650::init(vpViper650::vpToolType tool, const std::string &filename)
380 {
381  this->setToolType(tool);
382  this->parseConfigFile(filename.c_str());
383 }
384 
401 {
402  this->setToolType(tool);
403  this->set_eMc(eMc_);
404 }
405 
414 void vpViper650::parseConfigFile(const std::string &filename)
415 {
416  vpRxyzVector erc_; // eMc rotation
417  vpTranslationVector etc_; // eMc translation
418 
419  std::ifstream fdconfig(filename.c_str(), std::ios::in);
420 
421  if (!fdconfig.is_open()) {
422  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
423  filename.c_str());
424  }
425 
426  std::string line;
427  int lineNum = 0;
428  bool get_erc = false;
429  bool get_etc = false;
430  int code;
431 
432  while (std::getline(fdconfig, line)) {
433  lineNum++;
434  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
435  continue;
436  }
437  std::istringstream ss(line);
438  std::string key;
439  ss >> key;
440 
441  for (code = 0; nullptr != opt_viper650[code]; ++code) {
442  if (key.compare(opt_viper650[code]) == 0) {
443  break;
444  }
445  }
446 
447  switch (code) {
448  case 0:
449  break; // Nothing to do: camera name
450 
451  case 1: {
452  ss >> erc_[0] >> erc_[1] >> erc_[2];
453 
454  // Convert rotation from degrees to radians
455  erc_ = erc_ * M_PI / 180.0;
456  get_erc = true;
457  break;
458  }
459 
460  case 2: {
461  ss >> etc_[0] >> etc_[1] >> etc_[2];
462  get_etc = true;
463  break;
464  }
465 
466  default:
467  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
468  filename.c_str(), lineNum));
469  }
470  }
471  fdconfig.close();
472 
473  // Compute the eMc matrix from the translations and rotations
474  if (get_etc && get_erc) {
475  this->set_eMc(etc_, erc_);
476  } else {
478  "Could not read translation and rotation "
479  "parameters from config file %s",
480  filename.c_str());
481  }
482 }
483 
553 void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
554  const unsigned int &image_height) const
555 {
556 #if defined(VISP_HAVE_VIPER650_DATA)
557  vpXmlParserCamera parser;
558  switch (getToolType()) {
560  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
561  << std::endl
562  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
564  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
565  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
566  }
567  break;
568  }
570  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
571  << std::endl
572  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
574  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
575  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
576  }
577  break;
578  }
580  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
581  << std::endl
582  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
584  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
585  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
586  }
587  break;
588  }
590  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
591  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
593  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
594  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
595  }
596  break;
597  }
599  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
600  }
601  default: {
602  vpERROR_TRACE("This error should not occur!");
603  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
604  // "que les specs de la classe ont ete modifiee, "
605  // "et que le code n'a pas ete mis a jour "
606  // "correctement.");
607  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
608  // "vpViper650::vpViper650ToolType, et controlez que "
609  // "tous les cas ont ete pris en compte dans la "
610  // "fonction init(camera).");
611  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
612  }
613  }
614 #else
615  // Set default parameters
616  switch (getToolType()) {
618  // Set default intrinsic camera parameters for 640x480 images
619  if (image_width == 640 && image_height == 480) {
620  std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
621  << std::endl;
622  switch (this->projModel) {
624  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
625  break;
627  cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
628  break;
631  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
632  break;
633  }
634  } else {
635  vpTRACE("Cannot get default intrinsic camera parameters for this image "
636  "resolution");
637  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
638  }
639  break;
640  }
643  // Set default intrinsic camera parameters for 640x480 images
644  if (image_width == 640 && image_height == 480) {
645  std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
646  << std::endl;
647  switch (this->projModel) {
649  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
650  break;
652  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
653  break;
656  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
657  break;
658  }
659  } else {
660  vpTRACE("Cannot get default intrinsic camera parameters for this image "
661  "resolution");
662  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
663  }
664  break;
665  }
667  // Set default intrinsic camera parameters for 640x480 images
668  if (image_width == 640 && image_height == 480) {
669  std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\""
670  << std::endl;
671  switch (this->projModel) {
673  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
674  break;
676  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
677  break;
680  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
681  break;
682  }
683  } else {
684  vpTRACE("Cannot get default intrinsic camera parameters for this image "
685  "resolution");
686  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
687  }
688  break;
689  }
691  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
692  }
693  }
694 #endif
695  return;
696 }
697 
759 {
760  getCameraParameters(cam, I.getWidth(), I.getHeight());
761 }
824 {
825  getCameraParameters(cam, I.getWidth(), I.getHeight());
826 }
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 emitted by ViSP classes.
Definition: vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:85
@ notImplementedError
Not implemented.
Definition: vpException.h:81
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:240
unsigned int getHeight() const
Definition: vpImage.h:182
static double rad(double deg)
Definition: vpMath.h:127
Error that can be emitted by the vpRobot class and its derivatives.
@ readingParametersError
Cannot parse parameters.
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:176
Class that consider the case of a translation vector.
Modelization of the ADEPT Viper 650 robot.
Definition: vpViper650.h:93
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:104
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper650.h:112
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper650.h:166
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:102
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper650.h:127
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:103
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper650.h:150
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:105
void parseConfigFile(const std::string &filename)
Definition: vpViper650.cpp:414
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpViper650.cpp:553
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpViper650.h:114
void init(void)
Definition: vpViper650.cpp:132
static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:99
void setToolType(vpViper650::vpToolType tool)
Set the current tool type.
Definition: vpViper650.h:159
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper650.h:118
@ TOOL_MARLIN_F033C_CAMERA
Definition: vpViper650.h:119
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition: vpViper650.h:121
@ TOOL_GENERIC_CAMERA
Definition: vpViper650.h:122
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper650.h:120
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper650.h:111
static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:100
static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:101
static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:98
static const std::string CONST_CAMERA_FILENAME
Definition: vpViper650.h:106
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
Definition: vpViper650.h:113
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1212
vpTranslationVector etc
Definition: vpViper.h:156
double d6
for joint 6
Definition: vpViper.h:164
double a3
for joint 3
Definition: vpViper.h:162
double d4
for joint 4
Definition: vpViper.h:163
vpColVector joint_max
Definition: vpViper.h:169
double c56
Mechanical coupling between joint 5 and joint 6.
Definition: vpViper.h:166
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:154
double a1
Definition: vpViper.h:160
vpRxyzVector erc
Definition: vpViper.h:157
vpColVector joint_min
Definition: vpViper.h:170
double a2
for joint 2
Definition: vpViper.h:161
double d1
for joint 1
Definition: vpViper.h:160
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:411
#define vpERROR_TRACE
Definition: vpDebug.h:388