Visual Servoing Platform version 3.5.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
52static 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
84const 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
89const char *const vpViper650::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
90const char *const vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
91const char *const vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
92const 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
150void 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 }
345 this->eMc.buildFrom(etc, eRc);
346#endif // VISP_HAVE_VIPER650_DATA
347
348 setToolType(tool);
349 return;
350}
351
382void 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
417void 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 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 } else {
481 "Could not read translation and rotation "
482 "parameters from config file %s",
483 filename.c_str());
484 }
485}
486
556void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
557 const unsigned int &image_height) const
558{
559#if defined(VISP_HAVE_VIPER650_DATA)
560 vpXmlParserCamera parser;
561 switch (getToolType()) {
563 std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
564 << std::endl
565 << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
567 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
568 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
569 }
570 break;
571 }
573 std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
574 << std::endl
575 << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
577 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
578 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
579 }
580 break;
581 }
583 std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
584 << std::endl
585 << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
587 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
588 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
589 }
590 break;
591 }
593 std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
594 << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
596 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
597 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
598 }
599 break;
600 }
602 throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
603 }
604 default: {
605 vpERROR_TRACE("This error should not occur!");
606 // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
607 // "que les specs de la classe ont ete modifiee, "
608 // "et que le code n'a pas ete mis a jour "
609 // "correctement.");
610 // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
611 // "vpViper650::vpViper650ToolType, et controlez que "
612 // "tous les cas ont ete pris en compte dans la "
613 // "fonction init(camera).");
614 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
615 }
616 }
617#else
618 // Set default parameters
619 switch (getToolType()) {
621 // Set default intrinsic camera parameters for 640x480 images
622 if (image_width == 640 && image_height == 480) {
623 std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
624 << std::endl;
625 switch (this->projModel) {
627 cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
628 break;
630 cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
631 break;
633 throw vpException(vpException::notImplementedError, "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 \"" << vpViper650::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;
657 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
658 break;
659 }
660 } else {
661 vpTRACE("Cannot get default intrinsic camera parameters for this image "
662 "resolution");
663 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
664 }
665 break;
666 }
668 // Set default intrinsic camera parameters for 640x480 images
669 if (image_width == 640 && image_height == 480) {
670 std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\""
671 << std::endl;
672 switch (this->projModel) {
674 cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
675 break;
677 cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
678 break;
680 throw vpException(vpException::notImplementedError, "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
759{
761}
824{
826}
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:556
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