Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
49static const char *opt_viper850[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL};
50
51#ifdef VISP_HAVE_VIPER850_DATA
53 std::string(VISP_VIPER850_DATA_PATH) +
54 std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf");
55
57 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf");
58
60 std::string(VISP_VIPER850_DATA_PATH) +
61 std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf");
62
64 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf");
65
67 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/"
68 "const_eMc_schunk_gripper_without_distortion_Viper850."
69 "cnf");
70
72 std::string(VISP_VIPER850_DATA_PATH) +
73 std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf");
74
76 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper850.cnf");
77
79 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper850.cnf");
80
81const std::string vpViper850::CONST_CAMERA_FILENAME =
82 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml");
83
84#endif // VISP_HAVE_VIPER850_DATA
85
86const char *const vpViper850::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
87const char *const vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
88const char *const vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
89const char *const vpViper850::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
90
92
100 : tool_current(vpViper850::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
101
102{
103 // Denavit Hartenberg parameters
104 a1 = 0.075;
105 a2 = 0.365;
106 a3 = 0.090;
107 d1 = 0.335;
108 d4 = 0.405;
109 d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
110 c56 = -341.33 / 9102.22;
111
112 // Software joint limits in radians
113 joint_min[0] = vpMath::rad(-170);
114 joint_min[1] = vpMath::rad(-190);
115 joint_min[2] = vpMath::rad(-29);
116 joint_min[3] = vpMath::rad(-190);
117 joint_min[4] = vpMath::rad(-120);
118 joint_min[5] = vpMath::rad(-360);
119 joint_max[0] = vpMath::rad(170);
120 joint_max[1] = vpMath::rad(45);
121 joint_max[2] = vpMath::rad(256);
122 joint_max[3] = vpMath::rad(190);
123 joint_max[4] = vpMath::rad(120);
124 joint_max[5] = vpMath::rad(360);
125
126 init(); // Set the default tool
127}
128
134{
136 return;
137}
138
148void vpViper850::init(const std::string &camera_extrinsic_parameters)
149{
150 // vpTRACE ("Parse camera file \""%s\"".", camera_filename);
151 this->parseConfigFile(camera_extrinsic_parameters);
152
153 return;
154}
155
178{
179
180 this->projModel = proj_model;
181
182#ifdef VISP_HAVE_VIPER850_DATA
183 // Read the robot parameters from files
184 std::string filename_eMc;
185 switch (tool) {
187 switch (projModel) {
190 break;
193 break;
196 "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
197 break;
198 }
199 break;
200 }
202 switch (projModel) {
205 break;
208 break;
211 "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(
227 "Feature TOOL_SCHUNK_GRIPPER_CAMERA is not implemented for Kannala-Brandt projection model yet.");
228 break;
229 }
230 break;
231 }
233 switch (projModel) {
236 break;
239 break;
242 "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
243 break;
244 }
245 break;
246 }
249 "No predefined file available for a custom tool"
250 "You should use init(vpViper850::vpToolType, const std::string&) or"
251 "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
252 }
253 default: {
254 vpERROR_TRACE("This error should not occur!");
255 break;
256 }
257 }
258
259 this->init(filename_eMc);
260
261#else // VISP_HAVE_VIPER850_DATA
262
263 // Use here default values of the robot constant parameters.
264 switch (tool) {
266 switch (projModel) {
268 erc[0] = vpMath::rad(0.07); // rx
269 erc[1] = vpMath::rad(2.76); // ry
270 erc[2] = vpMath::rad(-91.50); // rz
271 etc[0] = -0.0453; // tx
272 etc[1] = 0.0005; // ty
273 etc[2] = 0.0728; // tz
274 break;
276 erc[0] = vpMath::rad(0.26); // rx
277 erc[1] = vpMath::rad(2.12); // ry
278 erc[2] = vpMath::rad(-91.31); // rz
279 etc[0] = -0.0444; // tx
280 etc[1] = -0.0005; // ty
281 etc[2] = 0.1022; // tz
282 break;
285 "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
286 break;
287 }
288 break;
289 }
292 switch (projModel) {
294 erc[0] = vpMath::rad(0.15); // rx
295 erc[1] = vpMath::rad(1.28); // ry
296 erc[2] = vpMath::rad(-90.8); // rz
297 etc[0] = -0.0456; // tx
298 etc[1] = -0.0013; // ty
299 etc[2] = 0.001; // tz
300 break;
302 erc[0] = vpMath::rad(0.72); // rx
303 erc[1] = vpMath::rad(2.10); // ry
304 erc[2] = vpMath::rad(-90.5); // rz
305 etc[0] = -0.0444; // tx
306 etc[1] = -0.0012; // ty
307 etc[2] = 0.078; // tz
308 break;
311 "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
312 break;
313 }
314 break;
315 }
317 // Set eMc to identity
318 switch (projModel) {
321 erc[0] = 0; // rx
322 erc[1] = 0; // ry
323 erc[2] = 0; // rz
324 etc[0] = 0; // tx
325 etc[1] = 0; // ty
326 etc[2] = 0; // tz
327 break;
330 "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
331 break;
332 }
333 break;
334 }
337 "No predefined parameters available for a custom tool"
338 "You should use init(vpViper850::vpToolType, const std::string&) or"
339 "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
340 }
341 }
343 this->eMc.buildFrom(etc, eRc);
344#endif // VISP_HAVE_VIPER850_DATA
345
346 setToolType(tool);
347 return;
348}
349
380void vpViper850::init(vpViper850::vpToolType tool, const std::string &filename)
381{
382 this->setToolType(tool);
383 this->parseConfigFile(filename.c_str());
384}
385
402{
403 this->setToolType(tool);
404 this->set_eMc(eMc_);
405}
406
415void vpViper850::parseConfigFile(const std::string &filename)
416{
417 vpRxyzVector erc_; // eMc rotation
418 vpTranslationVector etc_; // eMc translation
419
420 std::ifstream fdconfig(filename.c_str(), std::ios::in);
421
422 if (!fdconfig.is_open()) {
423 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
424 filename.c_str());
425 }
426
427 std::string line;
428 int lineNum = 0;
429 bool get_erc = false;
430 bool get_etc = false;
431 int code;
432
433 while (std::getline(fdconfig, line)) {
434 lineNum++;
435 if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
436 continue;
437 }
438 std::istringstream ss(line);
439 std::string key;
440 ss >> key;
441
442 for (code = 0; NULL != opt_viper850[code]; ++code) {
443 if (key.compare(opt_viper850[code]) == 0) {
444 break;
445 }
446 }
447
448 switch (code) {
449 case 0:
450 break; // Nothing to do: camera name
451
452 case 1: {
453 ss >> erc_[0] >> erc_[1] >> erc_[2];
454
455 // Convert rotation from degrees to radians
456 erc_ = erc_ * M_PI / 180.0;
457 get_erc = true;
458 break;
459 }
460
461 case 2: {
462 ss >> etc_[0] >> etc_[1] >> etc_[2];
463 get_etc = true;
464 break;
465 }
466
467 default:
468 throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
469 filename.c_str(), lineNum));
470 }
471 }
472
473 fdconfig.close();
474
475 // Compute the eMc matrix from the translations and rotations
476 if (get_etc && get_erc) {
477 this->set_eMc(etc_, erc_);
478 } else {
480 "Could not read translation and rotation "
481 "parameters from config file %s",
482 filename.c_str());
483 }
484}
485
555void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
556 const unsigned int &image_height) const
557{
558#if defined(VISP_HAVE_VIPER850_DATA)
559 vpXmlParserCamera parser;
560 switch (getToolType()) {
562 std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
563 << std::endl
564 << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
566 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
567 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
568 }
569 break;
570 }
572 std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
573 << std::endl
574 << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
576 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
577 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
578 }
579 break;
580 }
582 std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
583 << std::endl
584 << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
586 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
587 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
588 }
589 break;
590 }
592 std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
593 << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
595 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
596 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
597 }
598 break;
599 }
601 throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
602 }
603 default: {
604 vpERROR_TRACE("This error should not occur!");
605 // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
606 // "que les specs de la classe ont ete modifiee, "
607 // "et que le code n'a pas ete mis a jour "
608 // "correctement.");
609 // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
610 // "vpViper850::vpViper850ToolType, et controlez que "
611 // "tous les cas ont ete pris en compte dans la "
612 // "fonction init(camera).");
613 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
614 }
615 }
616#else
617 // Set default parameters
618 switch (getToolType()) {
620 // Set default intrinsic camera parameters for 640x480 images
621 if (image_width == 640 && image_height == 480) {
622 std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
623 << std::endl;
624 switch (this->projModel) {
626 cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
627 break;
629 cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
630 break;
633 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
634 break;
635 }
636 } else {
637 vpTRACE("Cannot get default intrinsic camera parameters for this image "
638 "resolution");
639 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
640 }
641 break;
642 }
645 // Set default intrinsic camera parameters for 640x480 images
646 if (image_width == 640 && image_height == 480) {
647 std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
648 << std::endl;
649 switch (this->projModel) {
651 cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
652 break;
654 cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
655 break;
658 "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 \"" << vpViper850::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;
682 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
683 break;
684 }
685 } else {
686 vpTRACE("Cannot get default intrinsic camera parameters for this image "
687 "resolution");
688 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
689 }
690 break;
691 }
693 throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
694 }
695 default:
696 vpERROR_TRACE("This error should not occur!");
697 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
698 }
699#endif
700 return;
701}
702
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static double rad(double deg)
Definition vpMath.h:116
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.
Class that consider the case of a translation vector.
Modelisation of the ADEPT Viper 850 robot.
Definition vpViper850.h:101
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition vpViper850.h:120
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition vpViper850.h:167
static const std::string CONST_CAMERA_FILENAME
Definition vpViper850.h:114
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition vpViper850.h:134
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
Definition vpViper850.h:121
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition vpViper850.h:113
void parseConfigFile(const std::string &filename)
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition vpViper850.h:110
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
Definition vpViper850.h:111
static const char *const CONST_GENERIC_CAMERA_NAME
Definition vpViper850.h:122
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
vpCameraParameters::vpCameraParametersProjType projModel
Definition vpViper850.h:174
static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
Definition vpViper850.h:107
static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
Definition vpViper850.h:108
vpToolType getToolType() const
Get the current tool type.
Definition vpViper850.h:158
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition vpViper850.h:125
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition vpViper850.h:128
@ TOOL_PTGREY_FLEA2_CAMERA
Definition vpViper850.h:127
@ TOOL_MARLIN_F033C_CAMERA
Definition vpViper850.h:126
@ TOOL_GENERIC_CAMERA
Definition vpViper850.h:129
static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
Definition vpViper850.h:106
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition vpViper850.h:119
static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
Definition vpViper850.h:109
void init(void)
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition vpViper850.h:112
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