Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
49static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL};
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
81const 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
86const char *const vpViper650::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
87const char *const vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
88const char *const vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
89const 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
147void 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 }
342 this->eMc.buildFrom(etc, eRc);
343#endif // VISP_HAVE_VIPER650_DATA
344
345 setToolType(tool);
346 return;
347}
348
379void 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
414void 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; NULL != 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
553void 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
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 650 robot.
Definition vpViper650.h:100
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition vpViper650.h:111
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition vpViper650.h:119
vpCameraParameters::vpCameraParametersProjType projModel
Definition vpViper650.h:173
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition vpViper650.h:109
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition vpViper650.h:133
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
Definition vpViper650.h:110
vpToolType getToolType() const
Get the current tool type.
Definition vpViper650.h:157
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition vpViper650.h:112
void parseConfigFile(const std::string &filename)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
static const char *const CONST_GENERIC_CAMERA_NAME
Definition vpViper650.h:121
void init(void)
static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
Definition vpViper650.h:106
void setToolType(vpViper650::vpToolType tool)
Set the current tool type.
Definition vpViper650.h:166
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition vpViper650.h:124
@ TOOL_MARLIN_F033C_CAMERA
Definition vpViper650.h:125
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition vpViper650.h:127
@ TOOL_GENERIC_CAMERA
Definition vpViper650.h:128
@ TOOL_PTGREY_FLEA2_CAMERA
Definition vpViper650.h:126
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition vpViper650.h:118
static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
Definition vpViper650.h:107
static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
Definition vpViper650.h:108
static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
Definition vpViper650.h:105
static const std::string CONST_CAMERA_FILENAME
Definition vpViper650.h:113
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
Definition vpViper650.h:120
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