Visual Servoing Platform  version 3.6.1 under development (2024-06-19)
vpViper850.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 850 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/vpViper850.h>
48 
49 static const char *opt_viper850[] = { "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr };
50 
52 #ifdef VISP_HAVE_VIPER850_DATA
54 std::string(VISP_VIPER850_DATA_PATH) +
55 std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf");
56 
58 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf");
59 
61 std::string(VISP_VIPER850_DATA_PATH) +
62 std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf");
63 
65 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf");
66 
68 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/"
69  "const_eMc_schunk_gripper_without_distortion_Viper850."
70  "cnf");
71 
73 std::string(VISP_VIPER850_DATA_PATH) +
74 std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf");
75 
77 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper850.cnf");
78 
80 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper850.cnf");
81 
82 const std::string vpViper850::CONST_CAMERA_FILENAME =
83 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml");
84 
85 #endif // VISP_HAVE_VIPER850_DATA
86 
87 const char *const vpViper850::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
88 const char *const vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
89 const char *const vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
90 const char *const vpViper850::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
91 
93 
101  : tool_current(vpViper850::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
102 
103 {
104  // Denavit-Hartenberg parameters
105  a1 = 0.075;
106  a2 = 0.365;
107  a3 = 0.090;
108  d1 = 0.335;
109  d4 = 0.405;
110  d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
111  c56 = -341.33 / 9102.22;
112 
113  // Software joint limits in radians
114  joint_min[0] = vpMath::rad(-170);
115  joint_min[1] = vpMath::rad(-190);
116  joint_min[2] = vpMath::rad(-29);
117  joint_min[3] = vpMath::rad(-190);
118  joint_min[4] = vpMath::rad(-120);
119  joint_min[5] = vpMath::rad(-360);
120  joint_max[0] = vpMath::rad(170);
121  joint_max[1] = vpMath::rad(45);
122  joint_max[2] = vpMath::rad(256);
123  joint_max[3] = vpMath::rad(190);
124  joint_max[4] = vpMath::rad(120);
125  joint_max[5] = vpMath::rad(360);
126 
127  init(); // Set the default tool
128 }
129 
135 {
137  return;
138 }
139 
149 void vpViper850::init(const std::string &camera_extrinsic_parameters)
150 {
151  // vpTRACE ("Parse camera file \""%s\"".", camera_filename);
152  this->parseConfigFile(camera_extrinsic_parameters);
153 
154  return;
155 }
156 
179 {
180 
181  this->projModel = proj_model;
182 
183 #ifdef VISP_HAVE_VIPER850_DATA
184  // Read the robot parameters from files
185  std::string filename_eMc;
186  switch (tool) {
188  switch (projModel) {
191  break;
194  break;
197  "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;
212  "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
213  break;
214  }
215  break;
216  }
218  switch (projModel) {
221  break;
224  break;
226  throw vpException(
228  "Feature TOOL_SCHUNK_GRIPPER_CAMERA is not implemented for Kannala-Brandt projection model yet.");
229  break;
230  }
231  break;
232  }
234  switch (projModel) {
237  break;
240  break;
243  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
244  break;
245  }
246  break;
247  }
250  "No predefined file available for a custom tool"
251  "You should use init(vpViper850::vpToolType, const std::string&) or"
252  "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
253  }
254  default: {
255  vpERROR_TRACE("This error should not occur!");
256  break;
257  }
258  }
259 
260  this->init(filename_eMc);
261 
262 #else // VISP_HAVE_VIPER850_DATA
263 
264  // Use here default values of the robot constant parameters.
265  switch (tool) {
267  switch (projModel) {
269  erc[0] = vpMath::rad(0.07); // rx
270  erc[1] = vpMath::rad(2.76); // ry
271  erc[2] = vpMath::rad(-91.50); // rz
272  etc[0] = -0.0453; // tx
273  etc[1] = 0.0005; // ty
274  etc[2] = 0.0728; // tz
275  break;
277  erc[0] = vpMath::rad(0.26); // rx
278  erc[1] = vpMath::rad(2.12); // ry
279  erc[2] = vpMath::rad(-91.31); // rz
280  etc[0] = -0.0444; // tx
281  etc[1] = -0.0005; // ty
282  etc[2] = 0.1022; // tz
283  break;
286  "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
287  break;
288  }
289  break;
290  }
293  switch (projModel) {
295  erc[0] = vpMath::rad(0.15); // rx
296  erc[1] = vpMath::rad(1.28); // ry
297  erc[2] = vpMath::rad(-90.8); // rz
298  etc[0] = -0.0456; // tx
299  etc[1] = -0.0013; // ty
300  etc[2] = 0.001; // tz
301  break;
303  erc[0] = vpMath::rad(0.72); // rx
304  erc[1] = vpMath::rad(2.10); // ry
305  erc[2] = vpMath::rad(-90.5); // rz
306  etc[0] = -0.0444; // tx
307  etc[1] = -0.0012; // ty
308  etc[2] = 0.078; // tz
309  break;
312  "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
313  break;
314  }
315  break;
316  }
318  // Set eMc to identity
319  switch (projModel) {
322  erc[0] = 0; // rx
323  erc[1] = 0; // ry
324  erc[2] = 0; // rz
325  etc[0] = 0; // tx
326  etc[1] = 0; // ty
327  etc[2] = 0; // tz
328  break;
331  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
332  break;
333  }
334  break;
335  }
338  "No predefined parameters available for a custom tool"
339  "You should use init(vpViper850::vpToolType, const std::string&) or"
340  "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
341  }
342  }
343  vpRotationMatrix eRc(erc);
344  this->eMc.build(etc, eRc);
345 #endif // VISP_HAVE_VIPER850_DATA
346 
347  setToolType(tool);
348  return;
349 }
350 
381 void vpViper850::init(vpViper850::vpToolType tool, const std::string &filename)
382 {
383  this->setToolType(tool);
384  this->parseConfigFile(filename.c_str());
385 }
386 
403 {
404  this->setToolType(tool);
405  this->set_eMc(eMc_);
406 }
407 
416 void vpViper850::parseConfigFile(const std::string &filename)
417 {
418  vpRxyzVector erc_; // eMc rotation
419  vpTranslationVector etc_; // eMc translation
420 
421  std::ifstream fdconfig(filename.c_str(), std::ios::in);
422 
423  if (!fdconfig.is_open()) {
424  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
425  filename.c_str());
426  }
427 
428  std::string line;
429  int lineNum = 0;
430  bool get_erc = false;
431  bool get_etc = false;
432  int code;
433 
434  while (std::getline(fdconfig, line)) {
435  lineNum++;
436  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
437  continue;
438  }
439  std::istringstream ss(line);
440  std::string key;
441  ss >> key;
442 
443  for (code = 0; nullptr != opt_viper850[code]; ++code) {
444  if (key.compare(opt_viper850[code]) == 0) {
445  break;
446  }
447  }
448 
449  switch (code) {
450  case 0:
451  break; // Nothing to do: camera name
452 
453  case 1: {
454  ss >> erc_[0] >> erc_[1] >> erc_[2];
455 
456  // Convert rotation from degrees to radians
457  erc_ = erc_ * M_PI / 180.0;
458  get_erc = true;
459  break;
460  }
461 
462  case 2: {
463  ss >> etc_[0] >> etc_[1] >> etc_[2];
464  get_etc = true;
465  break;
466  }
467 
468  default:
469  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
470  filename.c_str(), lineNum));
471  }
472  }
473 
474  fdconfig.close();
475 
476  // Compute the eMc matrix from the translations and rotations
477  if (get_etc && get_erc) {
478  this->set_eMc(etc_, erc_);
479  }
480  else {
482  "Could not read translation and rotation "
483  "parameters from config file %s",
484  filename.c_str());
485  }
486 }
487 
557 void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
558  const unsigned int &image_height) const
559 {
560 #if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_PUGIXML)
561  vpXmlParserCamera parser;
562  switch (getToolType()) {
564  std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
565  << std::endl
566  << "from the XML file: \"" << vpViper850::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 \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
575  << std::endl
576  << "from the XML file: \"" << vpViper850::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 \"" << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
585  << std::endl
586  << "from the XML file: \"" << vpViper850::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 \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << 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  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  // "vpViper850::vpViper850ToolType, 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 \"" << vpViper850::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;
635  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
636  break;
637  }
638  }
639  else {
640  vpTRACE("Cannot get default intrinsic camera parameters for this image "
641  "resolution");
642  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
643  }
644  break;
645  }
648  // Set default intrinsic camera parameters for 640x480 images
649  if (image_width == 640 && image_height == 480) {
650  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
651  << std::endl;
652  switch (this->projModel) {
654  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
655  break;
657  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
658  break;
661  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
662  break;
663  }
664  }
665  else {
666  vpTRACE("Cannot get default intrinsic camera parameters for this image "
667  "resolution");
668  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
669  }
670  break;
671  }
673  // Set default intrinsic camera parameters for 640x480 images
674  if (image_width == 640 && image_height == 480) {
675  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\""
676  << std::endl;
677  switch (this->projModel) {
679  cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
680  break;
682  cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
683  break;
686  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
687  break;
688  }
689  }
690  else {
691  vpTRACE("Cannot get default intrinsic camera parameters for this image "
692  "resolution");
693  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
694  }
695  break;
696  }
698  throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
699  }
700  default:
701  vpERROR_TRACE("This error should not occur!");
702  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
703  }
704 #endif
705  return;
706 }
707 
769 {
770  getCameraParameters(cam, I.getWidth(), I.getHeight());
771 }
834 {
835  getCameraParameters(cam, I.getWidth(), I.getHeight());
836 }
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:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
@ notImplementedError
Not implemented.
Definition: vpException.h:69
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:185
static double rad(double deg)
Definition: vpMath.h:129
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:179
Class that consider the case of a translation vector.
Modelization of the ADEPT Viper 850 robot.
Definition: vpViper850.h:95
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:114
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:161
static const std::string CONST_CAMERA_FILENAME
Definition: vpViper850.h:108
static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:100
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:129
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
Definition: vpViper850.h:115
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:107
void parseConfigFile(const std::string &filename)
Definition: vpViper850.cpp:416
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:104
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:105
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpViper850.h:116
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpViper850.cpp:557
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:168
static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:101
static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:102
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:152
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:120
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition: vpViper850.h:123
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper850.h:122
@ TOOL_MARLIN_F033C_CAMERA
Definition: vpViper850.h:121
@ TOOL_GENERIC_CAMERA
Definition: vpViper850.h:124
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:113
static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:103
void init(void)
Definition: vpViper850.cpp:134
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:106
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1225
vpTranslationVector etc
Definition: vpViper.h:158
double d6
for joint 6
Definition: vpViper.h:166
double a3
for joint 3
Definition: vpViper.h:164
double d4
for joint 4
Definition: vpViper.h:165
vpColVector joint_max
Definition: vpViper.h:171
double c56
Mechanical coupling between joint 5 and joint 6.
Definition: vpViper.h:168
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:156
double a1
Definition: vpViper.h:162
vpRxyzVector erc
Definition: vpViper.h:159
vpColVector joint_min
Definition: vpViper.h:172
double a2
for joint 2
Definition: vpViper.h:163
double d1
for joint 1
Definition: vpViper.h:162
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:408
#define vpERROR_TRACE
Definition: vpDebug.h:385