Visual Servoing Platform  version 3.4.0
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 }
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:110
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:417
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpViper650.cpp:557
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