Visual Servoing Platform  version 3.4.1 under development (2021-04-20)
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;
197  throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
198  break;
199  }
200  break;
201  }
203  switch (projModel) {
206  break;
209  break;
211  throw vpException(vpException::notImplementedError, "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
212  break;
213  }
214  break;
215  }
217  switch (projModel) {
220  break;
223  break;
225  throw vpException(vpException::notImplementedError, "Feature TOOL_SCHUNK_GRIPPER_CAMERA is not implemented for Kannala-Brandt projection model yet.");
226  break;
227  }
228  break;
229  }
231  switch (projModel) {
234  break;
237  break;
239  throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
240  break;
241  }
242  break;
243  }
246  "No predefined file available for a custom tool"
247  "You should use init(vpViper650::vpToolType, const std::string&) or"
248  "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
249  }
250  default: {
251  vpERROR_TRACE("This error should not occur!");
252  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
253  // "que les specs de la classe ont ete modifiee, "
254  // "et que le code n'a pas ete mis a jour "
255  // "correctement.");
256  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
257  // "vpViper650::vpViper650ToolType, et controlez que "
258  // "tous les cas ont ete pris en compte dans la "
259  // "fonction init(camera).");
260  break;
261  }
262  }
263 
264  this->init(filename_eMc);
265 
266 #else // VISP_HAVE_VIPER650_DATA
267 
268  // Use here default values of the robot constant parameters.
269  switch (tool) {
271  switch (projModel) {
273  erc[0] = vpMath::rad(0.07); // rx
274  erc[1] = vpMath::rad(2.76); // ry
275  erc[2] = vpMath::rad(-91.50); // rz
276  etc[0] = -0.0453; // tx
277  etc[1] = 0.0005; // ty
278  etc[2] = 0.0728; // tz
279  break;
281  erc[0] = vpMath::rad(0.26); // rx
282  erc[1] = vpMath::rad(2.12); // ry
283  erc[2] = vpMath::rad(-91.31); // rz
284  etc[0] = -0.0444; // tx
285  etc[1] = -0.0005; // ty
286  etc[2] = 0.1022; // tz
287  break;
289  throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
290  break;
291  }
292  break;
293  }
296  switch (projModel) {
298  erc[0] = vpMath::rad(0.15); // rx
299  erc[1] = vpMath::rad(1.28); // ry
300  erc[2] = vpMath::rad(-90.8); // rz
301  etc[0] = -0.0456; // tx
302  etc[1] = -0.0013; // ty
303  etc[2] = 0.001; // tz
304  break;
306  erc[0] = vpMath::rad(0.72); // rx
307  erc[1] = vpMath::rad(2.10); // ry
308  erc[2] = vpMath::rad(-90.5); // rz
309  etc[0] = -0.0444; // tx
310  etc[1] = -0.0012; // ty
311  etc[2] = 0.078; // tz
312  break;
314  throw vpException(vpException::notImplementedError, "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
315  break;
316  }
317  break;
318  }
320  // Set eMc to identity
321  switch (projModel) {
324  erc[0] = 0; // rx
325  erc[1] = 0; // ry
326  erc[2] = 0; // rz
327  etc[0] = 0; // tx
328  etc[1] = 0; // ty
329  etc[2] = 0; // tz
330  break;
332  throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
333  break;
334  }
335  break;
336  }
339  "No predefined parameters available for a custom tool"
340  "You should use init(vpViper650::vpToolType, const std::string&) or"
341  "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
342  }
343  }
344  vpRotationMatrix eRc(erc);
345  this->eMc.buildFrom(etc, eRc);
346 #endif // VISP_HAVE_VIPER650_DATA
347 
348  setToolType(tool);
349  return;
350 }
351 
382 void vpViper650::init(vpViper650::vpToolType tool, const std::string &filename)
383 {
384  this->setToolType(tool);
385  this->parseConfigFile(filename.c_str());
386 }
387 
404 {
405  this->setToolType(tool);
406  this->set_eMc(eMc_);
407 }
408 
417 void vpViper650::parseConfigFile(const std::string &filename)
418 {
419  vpRxyzVector erc_; // eMc rotation
420  vpTranslationVector etc_; // eMc translation
421 
422  std::ifstream fdconfig(filename.c_str(), std::ios::in);
423 
424  if (!fdconfig.is_open()) {
425  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
426  filename.c_str());
427  }
428 
429  std::string line;
430  int lineNum = 0;
431  bool get_erc = false;
432  bool get_etc = false;
433  int code;
434 
435  while (std::getline(fdconfig, line)) {
436  lineNum++;
437  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
438  continue;
439  }
440  std::istringstream ss(line);
441  std::string key;
442  ss >> key;
443 
444  for (code = 0; NULL != opt_viper650[code]; ++code) {
445  if (key.compare(opt_viper650[code]) == 0) {
446  break;
447  }
448  }
449 
450  switch (code) {
451  case 0:
452  break; // Nothing to do: camera name
453 
454  case 1: {
455  ss >> erc_[0] >> erc_[1] >> erc_[2];
456 
457  // Convert rotation from degrees to radians
458  erc_ = erc_ * M_PI / 180.0;
459  get_erc = true;
460  break;
461  }
462 
463  case 2: {
464  ss >> etc[0] >> etc[1] >> etc[2];
465  get_etc = true;
466  break;
467  }
468 
469  default:
470  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
471  filename.c_str(), lineNum));
472  }
473  }
474 
475  fdconfig.close();
476 
477  // Compute the eMc matrix from the translations and rotations
478  if (get_etc && get_erc) {
479  this->set_eMc(etc_, erc_);
480  } else {
482  "Could not read translation and rotation "
483  "parameters from config file %s",
484  filename.c_str());
485  }
486 }
487 
557 void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
558  const unsigned int &image_height) const
559 {
560 #if defined(VISP_HAVE_VIPER650_DATA)
561  vpXmlParserCamera parser;
562  switch (getToolType()) {
564  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
565  << std::endl
566  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
568  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
569  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
570  }
571  break;
572  }
574  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
575  << std::endl
576  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
578  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
579  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
580  }
581  break;
582  }
584  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
585  << std::endl
586  << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
588  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
589  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
590  }
591  break;
592  }
594  std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
595  << "from the XML file: \"" << vpViper650::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  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
604  }
605  default: {
606  vpERROR_TRACE("This error should not occur!");
607  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
608  // "que les specs de la classe ont ete modifiee, "
609  // "et que le code n'a pas ete mis a jour "
610  // "correctement.");
611  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
612  // "vpViper650::vpViper650ToolType, et controlez que "
613  // "tous les cas ont ete pris en compte dans la "
614  // "fonction init(camera).");
615  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
616  }
617  }
618 #else
619  // Set default parameters
620  switch (getToolType()) {
622  // Set default intrinsic camera parameters for 640x480 images
623  if (image_width == 640 && image_height == 480) {
624  std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
625  << std::endl;
626  switch (this->projModel) {
628  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
629  break;
631  cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
632  break;
634  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
635  break;
636  }
637  } else {
638  vpTRACE("Cannot get default intrinsic camera parameters for this image "
639  "resolution");
640  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
641  }
642  break;
643  }
646  // Set default intrinsic camera parameters for 640x480 images
647  if (image_width == 640 && image_height == 480) {
648  std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
649  << std::endl;
650  switch (this->projModel) {
652  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
653  break;
655  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
656  break;
658  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
659  break;
660  }
661  } else {
662  vpTRACE("Cannot get default intrinsic camera parameters for this image "
663  "resolution");
664  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
665  }
666  break;
667  }
669  // Set default intrinsic camera parameters for 640x480 images
670  if (image_width == 640 && image_height == 480) {
671  std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\""
672  << std::endl;
673  switch (this->projModel) {
675  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
676  break;
678  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
679  break;
681  throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
682  break;
683  }
684  } else {
685  vpTRACE("Cannot get default intrinsic camera parameters for this image "
686  "resolution");
687  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
688  }
689  break;
690  }
692  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
693  }
694  }
695 #endif
696  return;
697 }
698 
760 {
761  getCameraParameters(cam, I.getWidth(), I.getHeight());
762 }
825 {
826  getCameraParameters(cam, I.getWidth(), I.getHeight());
827 }
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
double a3
for joint 3
Definition: vpViper.h:165
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:115
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:157
Error that can be emited by the vpRobot class and its derivates.
Implementation of an homogeneous matrix and operations on such kind of matrices.
#define vpERROR_TRACE
Definition: vpDebug.h:393
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper650.h:176
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:114
static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:110
error that can be emited by ViSP classes.
Definition: vpException.h:71
static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:111
vpRxyzVector erc
Definition: vpViper.h:160
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:113
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpViper650.cpp:557
static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:108
XML parser to load and save intrinsic camera parameters.
double a2
for joint 2
Definition: vpViper.h:164
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
Implementation of a rotation matrix and operations on such kind of matrices.
double d1
for joint 1
Definition: vpViper.h:163
double c56
Mechanical coupling between joint 5 and joint 6.
Definition: vpViper.h:169
double a1
Definition: vpViper.h:163
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper650.h:136
vpTranslationVector etc
Definition: vpViper.h:159
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1230
#define vpTRACE
Definition: vpDebug.h:416
vpColVector joint_max
Definition: vpViper.h:172
void parseConfigFile(const std::string &filename)
Definition: vpViper650.cpp:417
Generic class defining intrinsic camera parameters.
Modelisation of the ADEPT Viper 650 robot.
Definition: vpViper650.h:102
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper650.h:121
void init(void)
Definition: vpViper650.cpp:135
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper650.h:127
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper650.h:122
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
Definition: vpMath.h:110
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
Definition: vpViper650.h:123
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)
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
unsigned int getHeight() const
Definition: vpImage.h:188
static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
Definition: vpViper650.h:109
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
double d4
for joint 4
Definition: vpViper.h:166
void setToolType(vpViper650::vpToolType tool)
Set the current tool type.
Definition: vpViper650.h:169
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpViper650.h:124
unsigned int getWidth() const
Definition: vpImage.h:246
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper650.h:160
Class that consider the case of a translation vector.
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpViper650.h:112
static const std::string CONST_CAMERA_FILENAME
Definition: vpViper650.h:116
double d6
for joint 6
Definition: vpViper.h:167
vpColVector joint_min
Definition: vpViper.h:173