Visual Servoing Platform version 3.5.0
vpViper850.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 850 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/vpViper850.h>
51
52static const char *opt_viper850[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL};
53
54#ifdef VISP_HAVE_VIPER850_DATA
56 std::string(VISP_VIPER850_DATA_PATH) +
57 std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf");
58
60 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf");
61
63 std::string(VISP_VIPER850_DATA_PATH) +
64 std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf");
65
67 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf");
68
70 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/"
71 "const_eMc_schunk_gripper_without_distortion_Viper850."
72 "cnf");
73
75 std::string(VISP_VIPER850_DATA_PATH) +
76 std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf");
77
79 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper850.cnf");
80
82 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper850.cnf");
83
84const std::string vpViper850::CONST_CAMERA_FILENAME =
85 std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml");
86
87#endif // VISP_HAVE_VIPER850_DATA
88
89const char *const vpViper850::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
90const char *const vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
91const char *const vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
92const char *const vpViper850::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
93
95
103 : tool_current(vpViper850::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
104
105{
106 // Denavit Hartenberg parameters
107 a1 = 0.075;
108 a2 = 0.365;
109 a3 = 0.090;
110 d1 = 0.335;
111 d4 = 0.405;
112 d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
113 c56 = -341.33 / 9102.22;
114
115 // Software joint limits in radians
116 joint_min[0] = vpMath::rad(-170);
117 joint_min[1] = vpMath::rad(-190);
118 joint_min[2] = vpMath::rad(-29);
119 joint_min[3] = vpMath::rad(-190);
120 joint_min[4] = vpMath::rad(-120);
121 joint_min[5] = vpMath::rad(-360);
122 joint_max[0] = vpMath::rad(170);
123 joint_max[1] = vpMath::rad(45);
124 joint_max[2] = vpMath::rad(256);
125 joint_max[3] = vpMath::rad(190);
126 joint_max[4] = vpMath::rad(120);
127 joint_max[5] = vpMath::rad(360);
128
129 init(); // Set the default tool
130}
131
137{
139 return;
140}
141
151void vpViper850::init(const std::string &camera_extrinsic_parameters)
152{
153 // vpTRACE ("Parse camera file \""%s\"".", camera_filename);
154 this->parseConfigFile(camera_extrinsic_parameters);
155
156 return;
157}
158
181{
182
183 this->projModel = proj_model;
184
185#ifdef VISP_HAVE_VIPER850_DATA
186 // Read the robot parameters from files
187 std::string filename_eMc;
188 switch (tool) {
190 switch (projModel) {
193 break;
196 break;
198 throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
199 break;
200 }
201 break;
202 }
204 switch (projModel) {
207 break;
210 break;
212 throw vpException(vpException::notImplementedError, "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(vpException::notImplementedError, "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;
240 throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
241 break;
242 }
243 break;
244 }
247 "No predefined file available for a custom tool"
248 "You should use init(vpViper850::vpToolType, const std::string&) or"
249 "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
250 }
251 default: {
252 vpERROR_TRACE("This error should not occur!");
253 // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
254 // "que les specs de la classe ont ete modifiee, "
255 // "et que le code n'a pas ete mis a jour "
256 // "correctement.");
257 // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
258 // "vpViper850::vpViper850ToolType, et controlez que "
259 // "tous les cas ont ete pris en compte dans la "
260 // "fonction init(camera).");
261 break;
262 }
263 }
264
265 this->init(filename_eMc);
266
267#else // VISP_HAVE_VIPER850_DATA
268
269 // Use here default values of the robot constant parameters.
270 switch (tool) {
272 switch (projModel) {
274 erc[0] = vpMath::rad(0.07); // rx
275 erc[1] = vpMath::rad(2.76); // ry
276 erc[2] = vpMath::rad(-91.50); // rz
277 etc[0] = -0.0453; // tx
278 etc[1] = 0.0005; // ty
279 etc[2] = 0.0728; // tz
280 break;
282 erc[0] = vpMath::rad(0.26); // rx
283 erc[1] = vpMath::rad(2.12); // ry
284 erc[2] = vpMath::rad(-91.31); // rz
285 etc[0] = -0.0444; // tx
286 etc[1] = -0.0005; // ty
287 etc[2] = 0.1022; // tz
288 break;
290 throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
291 break;
292 }
293 break;
294 }
297 switch (projModel) {
299 erc[0] = vpMath::rad(0.15); // rx
300 erc[1] = vpMath::rad(1.28); // ry
301 erc[2] = vpMath::rad(-90.8); // rz
302 etc[0] = -0.0456; // tx
303 etc[1] = -0.0013; // ty
304 etc[2] = 0.001; // tz
305 break;
307 erc[0] = vpMath::rad(0.72); // rx
308 erc[1] = vpMath::rad(2.10); // ry
309 erc[2] = vpMath::rad(-90.5); // rz
310 etc[0] = -0.0444; // tx
311 etc[1] = -0.0012; // ty
312 etc[2] = 0.078; // tz
313 break;
315 throw vpException(vpException::notImplementedError, "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
316 break;
317 }
318 break;
319 }
321 // Set eMc to identity
322 switch (projModel) {
325 erc[0] = 0; // rx
326 erc[1] = 0; // ry
327 erc[2] = 0; // rz
328 etc[0] = 0; // tx
329 etc[1] = 0; // ty
330 etc[2] = 0; // tz
331 break;
333 throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
334 break;
335 }
336 break;
337 }
340 "No predefined parameters available for a custom tool"
341 "You should use init(vpViper850::vpToolType, const std::string&) or"
342 "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead");
343 }
344 }
346 this->eMc.buildFrom(etc, eRc);
347#endif // VISP_HAVE_VIPER850_DATA
348
349 setToolType(tool);
350 return;
351}
352
383void vpViper850::init(vpViper850::vpToolType tool, const std::string &filename)
384{
385 this->setToolType(tool);
386 this->parseConfigFile(filename.c_str());
387}
388
405{
406 this->setToolType(tool);
407 this->set_eMc(eMc_);
408}
409
418void vpViper850::parseConfigFile(const std::string &filename)
419{
420 vpRxyzVector erc_; // eMc rotation
421 vpTranslationVector etc_; // eMc translation
422
423 std::ifstream fdconfig(filename.c_str(), std::ios::in);
424
425 if (!fdconfig.is_open()) {
426 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
427 filename.c_str());
428 }
429
430 std::string line;
431 int lineNum = 0;
432 bool get_erc = false;
433 bool get_etc = false;
434 int code;
435
436 while (std::getline(fdconfig, line)) {
437 lineNum++;
438 if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
439 continue;
440 }
441 std::istringstream ss(line);
442 std::string key;
443 ss >> key;
444
445 for (code = 0; NULL != opt_viper850[code]; ++code) {
446 if (key.compare(opt_viper850[code]) == 0) {
447 break;
448 }
449 }
450
451 switch (code) {
452 case 0:
453 break; // Nothing to do: camera name
454
455 case 1: {
456 ss >> erc_[0] >> erc_[1] >> erc_[2];
457
458 // Convert rotation from degrees to radians
459 erc_ = erc_ * M_PI / 180.0;
460 get_erc = true;
461 break;
462 }
463
464 case 2: {
465 ss >> etc_[0] >> etc_[1] >> etc_[2];
466 get_etc = true;
467 break;
468 }
469
470 default:
471 throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
472 filename.c_str(), lineNum));
473 }
474 }
475
476 fdconfig.close();
477
478 // Compute the eMc matrix from the translations and rotations
479 if (get_etc && get_erc) {
480 this->set_eMc(etc_, erc_);
481 } else {
483 "Could not read translation and rotation "
484 "parameters from config file %s",
485 filename.c_str());
486 }
487}
488
558void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
559 const unsigned int &image_height) const
560{
561#if defined(VISP_HAVE_VIPER850_DATA)
562 vpXmlParserCamera parser;
563 switch (getToolType()) {
565 std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
566 << std::endl
567 << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
569 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
570 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
571 }
572 break;
573 }
575 std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
576 << std::endl
577 << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
579 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
580 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
581 }
582 break;
583 }
585 std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
586 << std::endl
587 << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
589 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
590 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
591 }
592 break;
593 }
595 std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
596 << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl;
598 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
599 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
600 }
601 break;
602 }
604 throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
605 }
606 default: {
607 vpERROR_TRACE("This error should not occur!");
608 // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
609 // "que les specs de la classe ont ete modifiee, "
610 // "et que le code n'a pas ete mis a jour "
611 // "correctement.");
612 // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
613 // "vpViper850::vpViper850ToolType, et controlez que "
614 // "tous les cas ont ete pris en compte dans la "
615 // "fonction init(camera).");
616 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
617 }
618 }
619#else
620 // Set default parameters
621 switch (getToolType()) {
623 // Set default intrinsic camera parameters for 640x480 images
624 if (image_width == 640 && image_height == 480) {
625 std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
626 << std::endl;
627 switch (this->projModel) {
629 cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
630 break;
632 cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
633 break;
635 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
636 break;
637 }
638 } else {
639 vpTRACE("Cannot get default intrinsic camera parameters for this image "
640 "resolution");
641 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
642 }
643 break;
644 }
647 // Set default intrinsic camera parameters for 640x480 images
648 if (image_width == 640 && image_height == 480) {
649 std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
650 << std::endl;
651 switch (this->projModel) {
653 cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
654 break;
656 cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
657 break;
659 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
660 break;
661 }
662 } else {
663 vpTRACE("Cannot get default intrinsic camera parameters for this image "
664 "resolution");
665 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
666 }
667 break;
668 }
670 // Set default intrinsic camera parameters for 640x480 images
671 if (image_width == 640 && image_height == 480) {
672 std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\""
673 << std::endl;
674 switch (this->projModel) {
676 cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
677 break;
679 cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
680 break;
682 throw vpException(vpException::notImplementedError, "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
764{
766}
829{
831}
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 850 robot.
Definition: vpViper850.h:104
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:123
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:170
static const std::string CONST_CAMERA_FILENAME
Definition: vpViper850.h:117
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:137
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
Definition: vpViper850.h:124
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:116
void parseConfigFile(const std::string &filename)
Definition: vpViper850.cpp:418
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:113
static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:114
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpViper850.h:125
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpViper850.cpp:558
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:177
static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:110
static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:111
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:161
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:128
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition: vpViper850.h:131
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper850.h:130
@ TOOL_MARLIN_F033C_CAMERA
Definition: vpViper850.h:129
@ TOOL_GENERIC_CAMERA
Definition: vpViper850.h:132
static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:109
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:122
static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
Definition: vpViper850.h:112
void init(void)
Definition: vpViper850.cpp:136
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpViper850.h:115
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