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 Irisa's Afma6 robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39 /*!
40
41 \file vpAfma6.cpp
42
43 Control of Irisa's gantry robot named Afma6.
44
45 */
46
47 #include <iostream>
48 #include <sstream>
49
50 #include <visp3/core/vpCameraParameters.h>
51 #include <visp3/core/vpDebug.h>
52 #include <visp3/core/vpRotationMatrix.h>
53 #include <visp3/core/vpRxyzVector.h>
54 #include <visp3/core/vpTranslationVector.h>
55 #include <visp3/core/vpVelocityTwistMatrix.h>
56 #include <visp3/core/vpXmlParserCamera.h>
57 #include <visp3/robot/vpAfma6.h>
58 #include <visp3/robot/vpRobotException.h>
59
60 /* ----------------------------------------------------------------------- */
61 /* --- STATIC ------------------------------------------------------------ */
62 /* ---------------------------------------------------------------------- */
63
64 static const char *opt_Afma6[] = {"JOINT_MAX", "JOINT_MIN", "LONG_56", "COUPL_56",
65 "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL};
66
67 #ifdef VISP_HAVE_AFMA6_DATA
68 const std::string vpAfma6::CONST_AFMA6_FILENAME =
69 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf");
70
71 const std::string vpAfma6::CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME =
72 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf");
73
74 const std::string vpAfma6::CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME =
75 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf");
76
77 const std::string vpAfma6::CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME =
78 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf");
79
80 const std::string vpAfma6::CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME =
81 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf");
82
83 const std::string vpAfma6::CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME =
84 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf");
85
86 const std::string vpAfma6::CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME =
87 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf");
88
89 const std::string vpAfma6::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME =
90 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf");
91
92 const std::string vpAfma6::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME =
93 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf");
94
95 const std::string vpAfma6::CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME =
96 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf");
97
98 const std::string vpAfma6::CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME =
99 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf");
100
101 const std::string vpAfma6::CONST_CAMERA_AFMA6_FILENAME =
102 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml");
103
104 #endif // VISP_HAVE_AFMA6_DATA
105
106 const char *const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
107 const char *const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
108 const char *const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
109 const char *const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
110 const char *const vpAfma6::CONST_INTEL_D435_CAMERA_NAME = "Intel-D435";
111
112 const vpAfma6::vpAfma6ToolType vpAfma6::defaultTool = TOOL_CCMOP;
113
114 const unsigned int vpAfma6::njoint = 6;
115
116 /*!
117
118 Default constructor.
119
120 */
vpAfma6()121 vpAfma6::vpAfma6()
122 : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(vpAfma6::defaultTool),
123 projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
124 {
125 // Set the default parameters in case of the config files are not available.
126
127 //
128 // Geometric model constant parameters
129 //
130 // coupling between join 5 and 6
131 this->_coupl_56 = 0.009091;
132 // distance between join 5 and 6
133 this->_long_56 = -0.06924;
134 // Camera extrinsic parameters: effector to camera frame
135 this->_eMc.eye(); // Default values are initialized ...
136 // ... in init (vpAfma6::vpAfma6ToolType tool,
137 // vpCameraParameters::vpCameraParametersProjType projModel)
138 // Maximal value of the joints
139 this->_joint_max[0] = 0.7001;
140 this->_joint_max[1] = 0.5201;
141 this->_joint_max[2] = 0.4601;
142 this->_joint_max[3] = 2.7301;
143 this->_joint_max[4] = 2.4801;
144 this->_joint_max[5] = 1.5901;
145 // Minimal value of the joints
146 this->_joint_min[0] = -0.6501;
147 this->_joint_min[1] = -0.6001;
148 this->_joint_min[2] = -0.5001;
149 this->_joint_min[3] = -2.7301;
150 this->_joint_min[4] = -0.1001;
151 this->_joint_min[5] = -1.5901;
152
153 init();
154 }
155
156 /*!
157
158 Initialize the robot with the default tool vpAfma6::defaultTool.
159 */
init(void)160 void vpAfma6::init(void)
161 {
162 this->init(vpAfma6::defaultTool);
163 return;
164 }
165
166 /*!
167
168 Read files containing the constant parameters related to the robot
169 kinematics and to the end-effector to camera transformation.
170
171 \param camera_extrinsic_parameters : Filename containing the constant
172 parameters of the robot kinematics \f$^e{\bf M}_c \f$ transformation.
173
174 \param camera_intrinsic_parameters : Filename containing the camera
175 extrinsic parameters.
176
177 */
init(const std::string & camera_extrinsic_parameters,const std::string & camera_intrinsic_parameters)178 void vpAfma6::init(const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters)
179 {
180 this->parseConfigFile(camera_extrinsic_parameters);
181
182 this->parseConfigFile(camera_intrinsic_parameters);
183 }
184
185 /*!
186
187 Set the type of tool attached to the robot and transformation
188 between the end-effector and the tool (\f$^e{\bf M}c\f$).
189 This last parameter is loaded from a file.
190
191 \param tool : Type of tool in use.
192
193 \param filename : Path of the configuration file containing the
194 transformation between the end-effector frame and the tool frame.
195
196 The configuration file should have the form below:
197
198 \code
199 # Start with any number of consecutive lines
200 # beginning with the symbol '#'
201 #
202 # The 3 following lines contain the name of the camera,
203 # the rotation parameters of the geometric transformation
204 # using the Euler angles in degrees with convention XYZ and
205 # the translation parameters expressed in meters
206 CAMERA CameraName
207 eMc_ROT_XYZ 10.0 -90.0 20.0
208 eMc_TRANS_XYZ 0.05 0.01 0.06
209 \endcode
210
211 \sa init(vpAfma6::vpToolType,
212 vpCameraParameters::vpCameraParametersProjType), init(vpAfma6::vpToolType,
213 const vpHomogeneousMatrix&)
214 */
init(vpAfma6::vpAfma6ToolType tool,const std::string & filename)215 void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
216 {
217 this->setToolType(tool);
218 this->parseConfigFile(filename.c_str());
219 }
220
221 /*!
222
223 Read files containing the constant parameters related to the robot
224 tools in order to set the end-effector to tool transformation.
225
226 \param camera_extrinsic_parameters : Filename containing the camera
227 extrinsic parameters.
228
229 */
init(const std::string & camera_extrinsic_parameters)230 void vpAfma6::init(const std::string &camera_extrinsic_parameters)
231 {
232 this->parseConfigFile(camera_extrinsic_parameters);
233 }
234
235 /*!
236
237 Set the type of tool attached to the robot and the transformation
238 between the end-effector and the tool (\f$^e{\bf M}c\f$).
239
240 \param tool : Type of tool in use.
241
242 \param eMc : Homogeneous matrix representation of the transformation
243 between the end-effector frame and the tool frame.
244
245 \sa init(vpAfma6::vpAfma6ToolType,
246 vpCameraParameters::vpCameraParametersProjType),
247 init(vpAfma6::vpAfma6ToolType, const std::string&)
248
249 */
init(vpAfma6::vpAfma6ToolType tool,const vpHomogeneousMatrix & eMc)250 void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc)
251 {
252 this->setToolType(tool);
253 this->set_eMc(eMc);
254 }
255
256 /*!
257
258 Get the constant parameters related to the robot kinematics and to
259 the end-effector to camera transformation (eMc) corresponding to the
260 camera extrinsic parameters. These last parameters depend on the
261 camera and projection model in use.
262
263 \warning If the macro VISP_HAVE_AFMA6_DATA is defined in vpConfig.h
264 this function reads the camera extrinsic parameters from the file
265 corresponding to the specified camera type and projection type.
266 Otherwise corresponding default parameters are loaded.
267
268 \param tool : Camera in use.
269
270 \param proj_model : Projection model of the camera.
271
272 */
init(vpAfma6::vpAfma6ToolType tool,vpCameraParameters::vpCameraParametersProjType proj_model)273 void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType proj_model)
274 {
275
276 this->projModel = proj_model;
277
278 #ifdef VISP_HAVE_AFMA6_DATA
279 // Read the robot parameters from files
280 std::string filename_eMc;
281 switch (tool) {
282 case vpAfma6::TOOL_CCMOP: {
283 switch (projModel) {
284 case vpCameraParameters::perspectiveProjWithoutDistortion:
285 filename_eMc = CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME;
286 break;
287 case vpCameraParameters::perspectiveProjWithDistortion:
288 filename_eMc = CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME;
289 break;
290 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
291 throw vpException(vpException::notImplementedError, "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
292 break;
293 }
294 break;
295 }
296 case vpAfma6::TOOL_GRIPPER: {
297 switch (projModel) {
298 case vpCameraParameters::perspectiveProjWithoutDistortion:
299 filename_eMc = CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME;
300 break;
301 case vpCameraParameters::perspectiveProjWithDistortion:
302 filename_eMc = CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME;
303 break;
304 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
305 throw vpException(vpException::notImplementedError, "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
306 break;
307 }
308 break;
309 }
310 case vpAfma6::TOOL_VACUUM: {
311 switch (projModel) {
312 case vpCameraParameters::perspectiveProjWithoutDistortion:
313 filename_eMc = CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME;
314 break;
315 case vpCameraParameters::perspectiveProjWithDistortion:
316 filename_eMc = CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME;
317 break;
318 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
319 throw vpException(vpException::notImplementedError, "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
320 break;
321 }
322 break;
323 }
324 case vpAfma6::TOOL_INTEL_D435_CAMERA: {
325 switch (projModel) {
326 case vpCameraParameters::perspectiveProjWithoutDistortion:
327 filename_eMc = CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME;
328 break;
329 case vpCameraParameters::perspectiveProjWithDistortion:
330 filename_eMc = CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME;
331 break;
332 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
333 throw vpException(vpException::notImplementedError, "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
334 break;
335 }
336 break;
337 }
338 case vpAfma6::TOOL_GENERIC_CAMERA: {
339 switch (projModel) {
340 case vpCameraParameters::perspectiveProjWithoutDistortion:
341 filename_eMc = CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
342 break;
343 case vpCameraParameters::perspectiveProjWithDistortion:
344 filename_eMc = CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
345 break;
346 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
347 throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
348 break;
349 }
350 break;
351 }
352 default: {
353 vpERROR_TRACE("This error should not occur!");
354 // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
355 // "que les specs de la classe ont ete modifiee, "
356 // "et que le code n'a pas ete mis a jour "
357 // "correctement.");
358 // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
359 // "vpAfma6::vpAfma6ToolType, et controlez que "
360 // "tous les cas ont ete pris en compte dans la "
361 // "fonction init(camera).");
362 break;
363 }
364 }
365
366 this->init(vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
367
368 #else // VISP_HAVE_AFMA6_DATA
369
370 // Use here default values of the robot constant parameters.
371 switch (tool) {
372 case vpAfma6::TOOL_CCMOP: {
373 switch (projModel) {
374 case vpCameraParameters::perspectiveProjWithoutDistortion:
375 _erc[0] = vpMath::rad(164.35); // rx
376 _erc[1] = vpMath::rad(89.64); // ry
377 _erc[2] = vpMath::rad(-73.05); // rz
378 _etc[0] = 0.0117; // tx
379 _etc[1] = 0.0033; // ty
380 _etc[2] = 0.2272; // tz
381 break;
382 case vpCameraParameters::perspectiveProjWithDistortion:
383 _erc[0] = vpMath::rad(33.54); // rx
384 _erc[1] = vpMath::rad(89.34); // ry
385 _erc[2] = vpMath::rad(57.83); // rz
386 _etc[0] = 0.0373; // tx
387 _etc[1] = 0.0024; // ty
388 _etc[2] = 0.2286; // tz
389 break;
390 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
391 throw vpException(vpException::notImplementedError, "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
392 break;
393 }
394 break;
395 }
396 case vpAfma6::TOOL_GRIPPER: {
397 switch (projModel) {
398 case vpCameraParameters::perspectiveProjWithoutDistortion:
399 _erc[0] = vpMath::rad(88.33); // rx
400 _erc[1] = vpMath::rad(72.07); // ry
401 _erc[2] = vpMath::rad(2.53); // rz
402 _etc[0] = 0.0783; // tx
403 _etc[1] = 0.1234; // ty
404 _etc[2] = 0.1638; // tz
405 break;
406 case vpCameraParameters::perspectiveProjWithDistortion:
407 _erc[0] = vpMath::rad(86.69); // rx
408 _erc[1] = vpMath::rad(71.93); // ry
409 _erc[2] = vpMath::rad(4.17); // rz
410 _etc[0] = 0.1034; // tx
411 _etc[1] = 0.1142; // ty
412 _etc[2] = 0.1642; // tz
413 break;
414 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
415 throw vpException(vpException::notImplementedError, "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
416 break;
417 }
418 break;
419 }
420 case vpAfma6::TOOL_VACUUM: {
421 switch (projModel) {
422 case vpCameraParameters::perspectiveProjWithoutDistortion:
423 _erc[0] = vpMath::rad(90.40); // rx
424 _erc[1] = vpMath::rad(75.11); // ry
425 _erc[2] = vpMath::rad(0.18); // rz
426 _etc[0] = 0.0038; // tx
427 _etc[1] = 0.1281; // ty
428 _etc[2] = 0.1658; // tz
429 break;
430 case vpCameraParameters::perspectiveProjWithDistortion:
431 _erc[0] = vpMath::rad(91.61); // rx
432 _erc[1] = vpMath::rad(76.17); // ry
433 _erc[2] = vpMath::rad(-0.98); // rz
434 _etc[0] = 0.0815; // tx
435 _etc[1] = 0.1162; // ty
436 _etc[2] = 0.1658; // tz
437 break;
438 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
439 throw vpException(vpException::notImplementedError, "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
440 break;
441 }
442 break;
443 }
444 case vpAfma6::TOOL_INTEL_D435_CAMERA: {
445 switch (projModel) {
446 case vpCameraParameters::perspectiveProjWithoutDistortion:
447 _erc[0] = vpMath::rad(-71.41); // rx
448 _erc[1] = vpMath::rad(89.49); // ry
449 _erc[2] = vpMath::rad(162.07); // rz
450 _etc[0] = 0.0038; // tx
451 _etc[1] = 0.1281; // ty
452 _etc[2] = 0.1658; // tz
453 break;
454 case vpCameraParameters::perspectiveProjWithDistortion:
455 _erc[0] = vpMath::rad(-52.79); // rx
456 _erc[1] = vpMath::rad(89.55); // ry
457 _erc[2] = vpMath::rad(143.34); // rz
458 _etc[0] = 0.0693; // tx
459 _etc[1] = -0.0297; // ty
460 _etc[2] = 0.1357; // tz
461 break;
462 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
463 throw vpException(vpException::notImplementedError, "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
464 break;
465 }
466 break;
467 }
468 case vpAfma6::TOOL_CUSTOM:
469 case vpAfma6::TOOL_GENERIC_CAMERA: {
470 switch (projModel) {
471 case vpCameraParameters::perspectiveProjWithoutDistortion:
472 case vpCameraParameters::perspectiveProjWithDistortion:
473 // set eMc to identity
474 _erc[0] = 0; // rx
475 _erc[1] = 0; // ry
476 _erc[2] = 0; // rz
477 _etc[0] = 0; // tx
478 _etc[1] = 0; // ty
479 _etc[2] = 0; // tz
480 break;
481 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
482 throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
483 break;
484 }
485 break;
486 }
487 }
488 vpRotationMatrix eRc(_erc);
489 this->_eMc.buildFrom(_etc, eRc);
490 #endif // VISP_HAVE_AFMA6_DATA
491
492 setToolType(tool);
493 return;
494 }
495
496 /*!
497
498 Compute the forward kinematics (direct geometric model) as an
499 homogeneous matrix.
500
501 By forward kinematics we mean here the position and the orientation
502 of the camera relative to the base frame given the articular positions of
503 all the six joints.
504
505 This method is the same than get_fMc(const vpColVector & q).
506
507 \param q : Articular position of the six joints: q[0], q[1], q[2]
508 correspond to the first 3 translations expressed in meter, while
509 q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
510 radians.
511
512 \return The homogeneous matrix corresponding to the direct geometric
513 model which expresses the transformation between the base frame and the
514 camera frame (fMc).
515
516 \sa get_fMc(const vpColVector & q)
517 \sa getInverseKinematics()
518
519 */
getForwardKinematics(const vpColVector & q) const520 vpHomogeneousMatrix vpAfma6::getForwardKinematics(const vpColVector &q) const
521 {
522 vpHomogeneousMatrix fMc;
523 fMc = get_fMc(q);
524
525 return fMc;
526 }
527
528 /*!
529
530 Compute the inverse kinematics (inverse geometric model).
531
532 By inverse kinematics we mean here the six articular values of the joint
533 positions given the position and the orientation of the camera frame
534 relative to the base frame.
535
536 \param fMc : Homogeneous matrix describing the transformation from
537 base frame to the camera frame.
538
539 \param q : In input, the current articular joint position of the
540 robot. In output, the solution of the inverse kinematics. Articular
541 position of the six joints: q[0], q[1], q[2] correspond to the first
542 3 translations expressed in meter, while q[3], q[4] and q[5]
543 correspond to the 3 succesives rotations expressed in radians.
544
545 \param nearest : true to return the nearest solution to q. false to
546 return the farest.
547
548 \param verbose : Activates printings when no solution is found.
549
550 \return The number of solutions (1 or 2) of the inverse geometric
551 model. O, if no solution can be found.
552
553 The code below shows how to compute the inverse geometric model:
554
555 \code
556 #include <visp3/core/vpColVector.h>
557 #include <visp3/core/vpHomogeneousMatrix.h>
558 #include <visp3/robot/vpRobotAfma6.h>
559
560 int main()
561 {
562 #ifdef VISP_HAVE_AFMA6
563 vpColVector q1(6), q2(6);
564 vpHomogeneousMatrix fMc;
565
566 vpRobotAfma6 robot;
567
568 // Get the current articular position of the robot
569 robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
570
571 // Compute the pose of the camera in the reference frame using the
572 // direct geometric model
573 fMc = robot.getForwardKinematics(q1);
574 // this is similar to fMc = robot.get_fMc(q1);
575 // or robot.get_fMc(q1, fMc);
576
577 // Compute the inverse geometric model
578 int nbsol; // number of solutions (0, 1 or 2) of the inverse geometric model
579 // get the nearest solution to the current articular position
580 nbsol = robot.getInverseKinematics(fMc, q1, true);
581
582 if (nbsol == 0)
583 std::cout << "No solution of the inverse geometric model " << std::endl;
584 else if (nbsol >= 1)
585 std::cout << "First solution: " << q1 << std::endl;
586
587 if (nbsol == 2) {
588 // Compute the other solution of the inverse geometric model
589 q2 = q1;
590 robot.getInverseKinematics(fMc, q2, false);
591 std::cout << "Second solution: " << q2 << std::endl;
592 }
593 #endif
594 }
595 \endcode
596
597 \sa getForwardKinematics()
598
599 */
getInverseKinematics(const vpHomogeneousMatrix & fMc,vpColVector & q,const bool & nearest,const bool & verbose) const600 int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest,
601 const bool &verbose) const
602 {
603 vpHomogeneousMatrix fMe;
604 double q_[2][6], d[2], t;
605 int ok[2];
606 double cord[6];
607
608 int nbsol = 0;
609
610 if (q.getRows() != njoint)
611 q.resize(6);
612
613 // for(unsigned int i=0;i<3;i++) {
614 // fMe[i][3] = fMc[i][3];
615 // for(int j=0;j<3;j++) {
616 // fMe[i][j] = 0.0;
617 // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
618 // fMe[i][3] -= fMe[i][j]*rpi[j][3];
619 // }
620 // }
621
622 // std::cout << "\n\nfMc: " << fMc;
623 // std::cout << "\n\neMc: " << _eMc;
624
625 fMe = fMc * this->_eMc.inverse();
626 // std::cout << "\n\nfMe: " << fMe;
627
628 if (fMe[2][2] >= .99999f) {
629 vpTRACE("singularity\n");
630 q_[0][4] = q_[1][4] = M_PI / 2.f;
631 t = atan2(fMe[0][0], fMe[0][1]);
632 q_[1][3] = q_[0][3] = q[3];
633 q_[1][5] = q_[0][5] = t - q_[0][3];
634
635 while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
636 /* -> a cause du couplage 4/5 */
637 {
638 q_[1][5] -= vpMath::rad(10);
639 q_[1][3] += vpMath::rad(10);
640 }
641 while (q_[1][5] <= this->_joint_min[5]) {
642 q_[1][5] += vpMath::rad(10);
643 q_[1][3] -= vpMath::rad(10);
644 }
645 } else if (fMe[2][2] <= -.99999) {
646 vpTRACE("singularity\n");
647 q_[0][4] = q_[1][4] = -M_PI / 2;
648 t = atan2(fMe[1][1], fMe[1][0]);
649 q_[1][3] = q_[0][3] = q[3];
650 q_[1][5] = q_[0][5] = q_[0][3] - t;
651 while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
652 /* -> a cause du couplage 4/5 */
653 {
654 q_[1][5] -= vpMath::rad(10);
655 q_[1][3] -= vpMath::rad(10);
656 }
657 while (q_[1][5] <= this->_joint_min[5]) {
658 q_[1][5] += vpMath::rad(10);
659 q_[1][3] += vpMath::rad(10);
660 }
661 } else {
662 q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
663 if (q_[0][3] >= 0.0)
664 q_[1][3] = q_[0][3] - M_PI;
665 else
666 q_[1][3] = q_[0][3] + M_PI;
667
668 q_[0][4] = asin(fMe[2][2]);
669 if (q_[0][4] >= 0.0)
670 q_[1][4] = M_PI - q_[0][4];
671 else
672 q_[1][4] = -M_PI - q_[0][4];
673
674 q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
675 if (q_[0][5] >= 0.0)
676 q_[1][5] = q_[0][5] - M_PI;
677 else
678 q_[1][5] = q_[0][5] + M_PI;
679 }
680 q_[0][0] = fMe[0][3] - this->_long_56 * cos(q_[0][3]);
681 q_[1][0] = fMe[0][3] - this->_long_56 * cos(q_[1][3]);
682 q_[0][1] = fMe[1][3] - this->_long_56 * sin(q_[0][3]);
683 q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]);
684 q_[0][2] = q_[1][2] = fMe[2][3];
685
686 /* prise en compte du couplage axes 5/6 */
687 q_[0][5] += this->_coupl_56 * q_[0][4];
688 q_[1][5] += this->_coupl_56 * q_[1][4];
689
690 for (int j = 0; j < 2; j++) {
691 ok[j] = 1;
692 // test is position is reachable
693 for (unsigned int i = 0; i < 6; i++) {
694 if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
695 if (verbose) {
696 if (i < 3)
697 std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < "
698 << this->_joint_max[i] << std::endl;
699 else
700 std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < "
701 << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
702 }
703 ok[j] = 0;
704 }
705 }
706 }
707 if (ok[0] == 0) {
708 if (ok[1] == 0) {
709 std::cout << "No solution..." << std::endl;
710 nbsol = 0;
711 return nbsol;
712 } else if (ok[1] == 1) {
713 for (unsigned int i = 0; i < 6; i++)
714 cord[i] = q_[1][i];
715 nbsol = 1;
716 }
717 } else {
718 if (ok[1] == 0) {
719 for (unsigned int i = 0; i < 6; i++)
720 cord[i] = q_[0][i];
721 nbsol = 1;
722 } else {
723 nbsol = 2;
724 // vpTRACE("2 solutions\n");
725 for (int j = 0; j < 2; j++) {
726 d[j] = 0.0;
727 for (unsigned int i = 3; i < 6; i++)
728 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
729 }
730 if (nearest == true) {
731 if (d[0] <= d[1])
732 for (unsigned int i = 0; i < 6; i++)
733 cord[i] = q_[0][i];
734 else
735 for (unsigned int i = 0; i < 6; i++)
736 cord[i] = q_[1][i];
737 } else {
738 if (d[0] <= d[1])
739 for (unsigned int i = 0; i < 6; i++)
740 cord[i] = q_[1][i];
741 else
742 for (unsigned int i = 0; i < 6; i++)
743 cord[i] = q_[0][i];
744 }
745 }
746 }
747 for (unsigned int i = 0; i < 6; i++)
748 q[i] = cord[i];
749
750 return nbsol;
751 }
752
753 /*!
754
755 Compute the forward kinematics (direct geometric model) as an
756 homogeneous matrix.
757
758 By forward kinematics we mean here the position and the orientation
759 of the camera relative to the base frame given the articular positions of
760 all the six joints.
761
762 This method is the same than getForwardKinematics(const vpColVector & q).
763
764 \param q : Articular position of the six joints: q[0], q[1], q[2]
765 correspond to the first 3 translations expressed in meter, while
766 q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
767 radians.
768
769 \return The homogeneous matrix corresponding to the direct geometric
770 model which expresses the transformation between the base frame and the
771 camera frame (fMc).
772
773 \sa getForwardKinematics(const vpColVector & q)
774 */
get_fMc(const vpColVector & q) const775 vpHomogeneousMatrix vpAfma6::get_fMc(const vpColVector &q) const
776 {
777 vpHomogeneousMatrix fMc;
778 get_fMc(q, fMc);
779
780 return fMc;
781 }
782
783 /*!
784
785 Compute the forward kinematics (direct geometric model) as an
786 homogeneous matrix.
787
788 By forward kinematics we mean here the position and the orientation
789 of the camera relative to the base frame given the articular positions of
790 all the six joints.
791
792 \param q : Articular joint position of the robot. q[0], q[1], q[2]
793 correspond to the first 3 translations expressed in meter, while
794 q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
795 radians.
796
797 \param fMc The homogeneous matrix corresponding to the direct geometric
798 model which expresses the transformation between the fix frame and the
799 camera frame (fMc).
800
801 */
get_fMc(const vpColVector & q,vpHomogeneousMatrix & fMc) const802 void vpAfma6::get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
803 {
804
805 // Compute the direct geometric model: fMe = transformation between
806 // fix and end effector frame.
807 vpHomogeneousMatrix fMe;
808
809 get_fMe(q, fMe);
810
811 fMc = fMe * this->_eMc;
812
813 return;
814 }
815
816 /*!
817
818 Compute the forward kinematics (direct geometric model) as an
819 homogeneous matrix.
820
821 By forward kinematics we mean here the position and the orientation
822 of the end effector with respect to the base frame given the
823 articular positions of all the six joints.
824
825 \param q : Articular joint position of the robot. q[0], q[1], q[2]
826 correspond to the first 3 translations expressed in meter, while
827 q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
828 radians.
829
830 \param fMe The homogeneous matrix corresponding to the direct geometric
831 model which expresses the transformation between the fix frame and the
832 end effector frame (fMe).
833
834 */
get_fMe(const vpColVector & q,vpHomogeneousMatrix & fMe) const835 void vpAfma6::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
836 {
837 double q0 = q[0]; // meter
838 double q1 = q[1]; // meter
839 double q2 = q[2]; // meter
840
841 /* Decouplage liaisons 2 et 3. */
842 double q5 = q[5] - this->_coupl_56 * q[4];
843
844 double c1 = cos(q[3]);
845 double s1 = sin(q[3]);
846 double c2 = cos(q[4]);
847 double s2 = sin(q[4]);
848 double c3 = cos(q5);
849 double s3 = sin(q5);
850
851 // Compute the direct geometric model: fMe = transformation betwee
852 // fix and end effector frame.
853 fMe[0][0] = s1 * s2 * c3 + c1 * s3;
854 fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
855 fMe[0][2] = -s1 * c2;
856 fMe[0][3] = q0 + this->_long_56 * c1;
857
858 fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
859 fMe[1][1] = c1 * s2 * s3 + s1 * c3;
860 fMe[1][2] = c1 * c2;
861 fMe[1][3] = q1 + this->_long_56 * s1;
862
863 fMe[2][0] = c2 * c3;
864 fMe[2][1] = -c2 * s3;
865 fMe[2][2] = s2;
866 fMe[2][3] = q2;
867
868 fMe[3][0] = 0;
869 fMe[3][1] = 0;
870 fMe[3][2] = 0;
871 fMe[3][3] = 1;
872
873 // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
874
875 return;
876 }
877
878 /*!
879
880 Get the geometric transformation between the camera frame and the
881 end-effector frame. This transformation is constant and correspond
882 to the extrinsic camera parameters estimated by calibration.
883
884 \param cMe : Transformation between the camera frame and the
885 end-effector frame.
886
887 */
get_cMe(vpHomogeneousMatrix & cMe) const888 void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
889 /*!
890
891 Get the geometric transformation between the end-effector frame and the
892 camera or tool frame. This transformation is constant and correspond
893 to the extrinsic camera parameters estimated by calibration.
894
895 \return Transformation between the end-effector frame and the
896 camera frame.
897
898 */
get_eMc() const899 vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
900
901 /*!
902
903 Get the twist transformation from camera frame to end-effector
904 frame. This transformation allows to compute a velocity expressed
905 in the end-effector frame into the camera frame.
906
907 \param cVe : Twist transformation.
908
909 */
get_cVe(vpVelocityTwistMatrix & cVe) const910 void vpAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
911 {
912 vpHomogeneousMatrix cMe;
913 get_cMe(cMe);
914
915 cVe.buildFrom(cMe);
916
917 return;
918 }
919
920 /*!
921
922 Get the robot jacobian expressed in the end-effector frame.
923
924 \param q : Articular joint position of the robot. q[0], q[1], q[2]
925 correspond to the first 3 translations expressed in meter, while
926 q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
927 radians.
928
929 \param eJe : Robot jacobian expressed in the end-effector frame.
930
931 */
get_eJe(const vpColVector & q,vpMatrix & eJe) const932 void vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
933 {
934
935 eJe.resize(6, 6);
936
937 double s4, c4, s5, c5, s6, c6;
938
939 s4 = sin(q[3]);
940 c4 = cos(q[3]);
941 s5 = sin(q[4]);
942 c5 = cos(q[4]);
943 s6 = sin(q[5] - this->_coupl_56 * q[4]);
944 c6 = cos(q[5] - this->_coupl_56 * q[4]);
945
946 eJe = 0;
947 eJe[0][0] = s4 * s5 * c6 + c4 * s6;
948 eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
949 eJe[0][2] = c5 * c6;
950 eJe[0][3] = -this->_long_56 * s5 * c6;
951
952 eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
953 eJe[1][1] = c4 * s5 * s6 + s4 * c6;
954 eJe[1][2] = -c5 * s6;
955 eJe[1][3] = this->_long_56 * s5 * s6;
956
957 eJe[2][0] = -s4 * c5;
958 eJe[2][1] = c4 * c5;
959 eJe[2][2] = s5;
960 eJe[2][3] = this->_long_56 * c5;
961
962 eJe[3][3] = c5 * c6;
963 eJe[3][4] = s6;
964
965 eJe[4][3] = -c5 * s6;
966 eJe[4][4] = c6;
967
968 eJe[5][3] = s5;
969 eJe[5][4] = -this->_coupl_56;
970 eJe[5][5] = 1;
971
972 return;
973 }
974
975 /*!
976
977 Get the robot jacobian expressed in the robot reference frame also
978 called fix frame.
979
980 \f[
981 {^f}J_e = \left(\begin{array}{cccccc}
982 1 & 0 & 0 & -Ls4 & 0 & 0 \\
983 0 & 1 & 0 & Lc4 & 0 & 0 \\
984 0 & 0 & 1 & 0 & 0 & 0 \\
985 0 & 0 & 0 & 0 & c4+\gamma s4c5 & -s4c5 \\
986 0 & 0 & 0 & 0 & s4-\gamma c4c5 & c4c5 \\
987 0 & 0 & 0 & 1 & -gamma s5 & s5 \\
988 \end{array}
989 \right)
990 \f]
991 where \f$\gamma\f$ is the coupling factor between join 5 and 6.
992
993 \param q : Articular joint position of the robot. q[0], q[1], q[2]
994 correspond to the first 3 translations expressed in meter, while
995 q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
996 radians.
997
998 \param fJe : Robot jacobian expressed in the robot reference frame.
999
1000 */
1001
get_fJe(const vpColVector & q,vpMatrix & fJe) const1002 void vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1003 {
1004
1005 fJe.resize(6, 6);
1006
1007 // block superieur gauche
1008 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1009
1010 double s4 = sin(q[3]);
1011 double c4 = cos(q[3]);
1012
1013 // block superieur droit
1014 fJe[0][3] = -this->_long_56 * s4;
1015 fJe[1][3] = this->_long_56 * c4;
1016
1017 double s5 = sin(q[4]);
1018 double c5 = cos(q[4]);
1019 // block inferieur droit
1020 fJe[3][4] = c4;
1021 fJe[3][5] = -s4 * c5;
1022 fJe[4][4] = s4;
1023 fJe[4][5] = c4 * c5;
1024 fJe[5][3] = 1;
1025 fJe[5][5] = s5;
1026
1027 // coupling between joint 5 and 6
1028 fJe[3][4] += this->_coupl_56 * s4 * c5;
1029 fJe[4][4] += -this->_coupl_56 * c4 * c5;
1030 fJe[5][4] += -this->_coupl_56 * s5;
1031
1032 return;
1033 }
1034
1035 /*!
1036 Get min joint values.
1037
1038 \return Minimal joint values for the 6 dof
1039 X,Y,Z,A,B,C. Translation X,Y,Z are expressed in meters. Rotation
1040 A,B,C in radians.
1041
1042 */
getJointMin() const1043 vpColVector vpAfma6::getJointMin() const
1044 {
1045 vpColVector qmin(6);
1046 for (unsigned int i = 0; i < 6; i++)
1047 qmin[i] = this->_joint_min[i];
1048 return qmin;
1049 }
1050
1051 /*!
1052 Get max joint values.
1053
1054 \return Maximal joint values for the 6 dof
1055 X,Y,Z,A,B,C. Translation X,Y,Z are expressed in meters. Rotation
1056 A,B,C in radians.
1057
1058 */
getJointMax() const1059 vpColVector vpAfma6::getJointMax() const
1060 {
1061 vpColVector qmax(6);
1062 for (unsigned int i = 0; i < 6; i++)
1063 qmax[i] = this->_joint_max[i];
1064 return qmax;
1065 }
1066
1067 /*!
1068
1069 Return the coupling factor between join 5 and 6.
1070
1071 \return Coupling factor between join 5 and 6.
1072 */
getCoupl56() const1073 double vpAfma6::getCoupl56() const { return _coupl_56; }
1074
1075 /*!
1076
1077 Return the distance between join 5 and 6.
1078
1079 \return Distance between join 5 and 6.
1080 */
getLong56() const1081 double vpAfma6::getLong56() const { return _long_56; }
1082
1083 /*!
1084
1085 This function gets the robot constant parameters from a file.
1086
1087 \param filename : File name containing the robot constant
1088 parameters, like max/min joint values, distance between 5 and 6 axis,
1089 coupling factor between axis 5 and 6, and the hand-to-eye homogeneous
1090 matrix.
1091
1092 */
parseConfigFile(const std::string & filename)1093 void vpAfma6::parseConfigFile(const std::string &filename)
1094 {
1095 vpRxyzVector erc; // eMc rotation
1096 vpTranslationVector etc; // eMc translation
1097
1098 std::ifstream fdconfig(filename.c_str(), std::ios::in);
1099
1100 if (!fdconfig.is_open()) {
1101 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
1102 filename.c_str());
1103 }
1104
1105 std::string line;
1106 int lineNum = 0;
1107 bool get_erc = false;
1108 bool get_etc = false;
1109 int code;
1110
1111 while (std::getline(fdconfig, line)) {
1112 lineNum++;
1113 if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
1114 continue;
1115 }
1116 std::istringstream ss(line);
1117 std::string key;
1118 ss >> key;
1119
1120 for (code = 0; NULL != opt_Afma6[code]; ++code) {
1121 if (key.compare(opt_Afma6[code]) == 0) {
1122 break;
1123 }
1124 }
1125
1126 switch (code) {
1127 case 0:
1128 ss >> this->_joint_max[0] >> this->_joint_max[1] >> this->_joint_max[2] >> this->_joint_max[3] >>
1129 this->_joint_max[4] >> this->_joint_max[5];
1130 break;
1131
1132 case 1:
1133 ss >> this->_joint_min[0] >> this->_joint_min[1] >> this->_joint_min[2] >> this->_joint_min[3] >>
1134 this->_joint_min[4] >> this->_joint_min[5];
1135 break;
1136
1137 case 2:
1138 ss >> this->_long_56;
1139 break;
1140
1141 case 3:
1142 ss >> this->_coupl_56;
1143 break;
1144
1145 case 4:
1146 break; // Nothing to do: camera name
1147
1148 case 5:
1149 ss >> erc[0] >> erc[1] >> erc[2];
1150
1151 // Convert rotation from degrees to radians
1152 erc = erc * M_PI / 180.0;
1153 get_erc = true;
1154 break;
1155
1156 case 6:
1157 ss >> etc[0] >> etc[1] >> etc[2];
1158 get_etc = true;
1159 break;
1160
1161 default:
1162 throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
1163 filename.c_str(), lineNum));
1164 }
1165 }
1166
1167 fdconfig.close();
1168
1169 // Compute the eMc matrix from the translations and rotations
1170 if (get_etc && get_erc) {
1171 _erc = erc;
1172 _etc = etc;
1173
1174 vpRotationMatrix eRc(_erc);
1175 this->_eMc.buildFrom(_etc, eRc);
1176 }
1177 }
1178
1179 /*!
1180
1181 Set the geometric transformation between the end-effector frame and
1182 the tool frame (commonly a camera).
1183
1184 \param eMc : Transformation between the end-effector frame
1185 and the tool frame.
1186 */
set_eMc(const vpHomogeneousMatrix & eMc)1187 void vpAfma6::set_eMc(const vpHomogeneousMatrix &eMc)
1188 {
1189 this->_eMc = eMc;
1190 this->_etc = eMc.getTranslationVector();
1191
1192 vpRotationMatrix R(eMc);
1193 this->_erc.buildFrom(R);
1194 }
1195
1196 /*!
1197 Get the current intrinsic camera parameters obtained by calibration.
1198
1199 \warning This method needs XML library to parse the file defined in
1200 vpAfma6::CONST_CAMERA_AFMA6_FILENAME and containing the camera
1201 parameters.
1202
1203 \warning Thid method needs also an access to the files containing the
1204 camera parameters in XML format. This access is available if
1205 VISP_HAVE_AFMA6_DATA macro is defined in include/visp3/core/vpConfig.h file.
1206
1207 - If VISP_HAVE_AFMA6_DATA is defined, this method gets the camera parameters
1208 from const_camera_Afma6.xml config file.
1209
1210 - If this macro is not defined, this method sets the camera parameters
1211 to default one.
1212
1213 \param cam : In output, camera parameters to fill.
1214 \param image_width : Image width used to compute camera calibration.
1215 \param image_height : Image height used to compute camera calibration.
1216
1217 The code below shows how to get the camera parameters of the camera
1218 attached to the robot.
1219
1220 \code
1221 #include <visp3/core/vpCameraParameters.h>
1222 #include <visp3/core/vpImage.h>
1223 #include <visp3/robot/vpRobotAfma6.h>
1224 #include <visp3/sensor/vp1394TwoGrabber.h>
1225
1226 int main()
1227 {
1228 #if defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_AFMA6)
1229 vpImage<unsigned char> I;
1230 vp1394TwoGrabber g;
1231
1232 // Acquire an image to update image structure
1233 g.acquire(I) ;
1234
1235 vpRobotAfma6 robot;
1236 vpCameraParameters cam ;
1237 // Get the intrinsic camera parameters depending on the image size
1238 // Camera parameters are read from
1239 // /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
1240 // if VISP_HAVE_AFMA6_DATA macro is defined in vpConfig.h file
1241 try {
1242 robot.getCameraParameters (cam, I.getWidth(), I.getHeight());
1243 }
1244 catch(...) {
1245 std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
1246 }
1247 std::cout << "Camera parameters: " << cam << std::endl;
1248 #endif
1249 }
1250 \endcode
1251
1252 \exception vpRobotException::readingParametersError : If the camera
1253 parameters are not found.
1254 */
1255
getCameraParameters(vpCameraParameters & cam,const unsigned int & image_width,const unsigned int & image_height) const1256 void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
1257 const unsigned int &image_height) const
1258 {
1259 #if defined(VISP_HAVE_AFMA6_DATA)
1260 vpXmlParserCamera parser;
1261 switch (getToolType()) {
1262 case vpAfma6::TOOL_CCMOP: {
1263 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1264 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1265 if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_CCMOP_CAMERA_NAME, projModel,
1266 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1267 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1268 }
1269 break;
1270 }
1271 case vpAfma6::TOOL_GRIPPER: {
1272 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1273 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1274 if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_GRIPPER_CAMERA_NAME, projModel,
1275 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1276 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1277 }
1278 break;
1279 }
1280 case vpAfma6::TOOL_VACUUM: {
1281 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1282 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1283 if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_VACUUM_CAMERA_NAME, projModel,
1284 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1285 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1286 }
1287 break;
1288 }
1289 case vpAfma6::TOOL_INTEL_D435_CAMERA: {
1290 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\"" << std::endl
1291 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1292 if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_VACUUM_CAMERA_NAME, projModel,
1293 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1294 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1295 }
1296 break;
1297 }
1298 case vpAfma6::TOOL_GENERIC_CAMERA: {
1299 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1300 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1301 if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_GENERIC_CAMERA_NAME, projModel,
1302 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1303 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1304 }
1305 break;
1306 }
1307 default: {
1308 vpERROR_TRACE("This error should not occur!");
1309 // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1310 // "que les specs de la classe ont ete modifiee, "
1311 // "et que le code n'a pas ete mis a jour "
1312 // "correctement.");
1313 // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1314 // "vpAfma6::vpAfma6ToolType, et controlez que "
1315 // "tous les cas ont ete pris en compte dans la "
1316 // "fonction init(camera).");
1317 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1318 }
1319 }
1320 #else
1321 // Set default parameters
1322 switch (getToolType()) {
1323 case vpAfma6::TOOL_CCMOP: {
1324 // Set default intrinsic camera parameters for 640x480 images
1325 if (image_width == 640 && image_height == 480) {
1326 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
1327 << std::endl;
1328 switch (this->projModel) {
1329 case vpCameraParameters::perspectiveProjWithoutDistortion:
1330 cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1331 break;
1332 case vpCameraParameters::perspectiveProjWithDistortion:
1333 cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1334 break;
1335 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
1336 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1337 break;
1338 }
1339 } else {
1340 vpTRACE("Cannot get default intrinsic camera parameters for this image "
1341 "resolution");
1342 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1343 }
1344 break;
1345 }
1346 case vpAfma6::TOOL_GRIPPER: {
1347 // Set default intrinsic camera parameters for 640x480 images
1348 if (image_width == 640 && image_height == 480) {
1349 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
1350 << std::endl;
1351 switch (this->projModel) {
1352 case vpCameraParameters::perspectiveProjWithoutDistortion:
1353 cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
1354 break;
1355 case vpCameraParameters::perspectiveProjWithDistortion:
1356 cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
1357 break;
1358 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
1359 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1360 break;
1361 }
1362 } else {
1363 vpTRACE("Cannot get default intrinsic camera parameters for this image "
1364 "resolution");
1365 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1366 }
1367 break;
1368 }
1369 case vpAfma6::TOOL_VACUUM: {
1370 // Set default intrinsic camera parameters for 640x480 images
1371 if (image_width == 640 && image_height == 480) {
1372 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\""
1373 << std::endl;
1374 switch (this->projModel) {
1375 case vpCameraParameters::perspectiveProjWithoutDistortion:
1376 cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1377 break;
1378 case vpCameraParameters::perspectiveProjWithDistortion:
1379 cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1380 break;
1381 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
1382 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1383 break;
1384 }
1385 } else {
1386 vpTRACE("Cannot get default intrinsic camera parameters for this image "
1387 "resolution");
1388 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1389 }
1390 break;
1391 }
1392 case vpAfma6::TOOL_INTEL_D435_CAMERA: {
1393 // Set default intrinsic camera parameters for 640x480 images
1394 if (image_width == 640 && image_height == 480) {
1395 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\""
1396 << std::endl;
1397 switch (this->projModel) {
1398 case vpCameraParameters::perspectiveProjWithoutDistortion:
1399 cam.initPersProjWithoutDistortion(605.4, 605.6, 328.6, 241.0);
1400 break;
1401 case vpCameraParameters::perspectiveProjWithDistortion:
1402 cam.initPersProjWithDistortion(611.8, 612.6, 327.8, 241.7, 0.0436, -0.04265);
1403 break;
1404 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
1405 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1406 break;
1407 }
1408 } else {
1409 vpTRACE("Cannot get default intrinsic camera parameters for this image "
1410 "resolution");
1411 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1412 }
1413 break;
1414 }
1415 case vpAfma6::TOOL_GENERIC_CAMERA: {
1416 // Set default intrinsic camera parameters for 640x480 images
1417 if (image_width == 640 && image_height == 480) {
1418 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\""
1419 << std::endl;
1420 switch (this->projModel) {
1421 case vpCameraParameters::perspectiveProjWithoutDistortion:
1422 cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1423 break;
1424 case vpCameraParameters::perspectiveProjWithDistortion:
1425 cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1426 break;
1427 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
1428 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1429 break;
1430 }
1431 } else {
1432 vpTRACE("Cannot get default intrinsic camera parameters for this image "
1433 "resolution");
1434 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1435 }
1436 break;
1437 }
1438 default:
1439 vpERROR_TRACE("This error should not occur!");
1440 break;
1441 }
1442 #endif
1443 return;
1444 }
1445
1446 /*!
1447 Get the current intrinsic camera parameters obtained by calibration.
1448
1449 Camera parameters are read from
1450 /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
1451
1452 \param cam : In output, camera parameters to fill.
1453 \param I : A B&W image send by the current camera in use.
1454
1455 \code
1456 #include <visp3/core/vpCameraParameters.h>
1457 #include <visp3/core/vpImage.h>
1458 #include <visp3/robot/vpRobotAfma6.h>
1459 #include <visp3/sensor/vp1394TwoGrabber.h>
1460
1461 int main()
1462 {
1463 #if defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_AFMA6)
1464 vpImage<unsigned char> I;
1465 vp1394TwoGrabber g;
1466
1467 // Acquire an image to update image structure
1468 g.acquire(I) ;
1469
1470 vpRobotAfma6 robot;
1471 vpCameraParameters cam ;
1472 // Get the intrinsic camera parameters depending on the image size
1473 try {
1474 robot.getCameraParameters (cam, I);
1475 }
1476 catch(...) {
1477 std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
1478 }
1479 std::cout << "Camera parameters: " << cam << std::endl;
1480 #endif
1481 }
1482 \endcode
1483
1484 \exception vpRobotException::readingParametersError : If the camera
1485 parameters are not found.
1486
1487 */
getCameraParameters(vpCameraParameters & cam,const vpImage<unsigned char> & I) const1488 void vpAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I) const
1489 {
1490 getCameraParameters(cam, I.getWidth(), I.getHeight());
1491 }
1492 /*!
1493 \brief Get the current intrinsic camera parameters obtained by calibration.
1494
1495 Camera parameters are read from
1496 /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
1497
1498 \param cam : In output, camera parameters to fill.
1499 \param I : A color image send by the current camera in use.
1500
1501 \code
1502 #include <visp3/core/vpCameraParameters.h>
1503 #include <visp3/core/vpImage.h>
1504 #include <visp3/robot/vpRobotAfma6.h>
1505 #include <visp3/sensor/vp1394TwoGrabber.h>
1506
1507 int main()
1508 {
1509 #if defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_AFMA6)
1510 vpImage<vpRGBa> I;
1511 vp1394TwoGrabber g;
1512
1513 // Acquire an image to update image structure
1514 g.acquire(I) ;
1515
1516 vpRobotAfma6 robot;
1517 vpCameraParameters cam ;
1518 // Get the intrinsic camera parameters depending on the image size
1519 try {
1520 robot.getCameraParameters (cam, I);
1521 }
1522 catch(...) {
1523 std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
1524 }
1525 std::cout << "Camera parameters: " << cam << std::endl;
1526 #endif
1527 }
1528 \endcode
1529
1530 \exception vpRobotException::readingParametersError : If the camera
1531 parameters are not found.
1532
1533 */
1534
getCameraParameters(vpCameraParameters & cam,const vpImage<vpRGBa> & I) const1535 void vpAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const
1536 {
1537 getCameraParameters(cam, I.getWidth(), I.getHeight());
1538 }
1539
1540 /*!
1541
1542 Print on the output stream \e os the robot parameters (joint
1543 min/max, distance between axis 5 and 6, coupling factor between axis
1544 5 and 6, hand-to-eye homogeneous matrix.
1545
1546 \param os : Output stream.
1547 \param afma6 : Robot parameters.
1548 */
operator <<(std::ostream & os,const vpAfma6 & afma6)1549 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6)
1550 {
1551 vpRotationMatrix eRc;
1552 afma6._eMc.extract(eRc);
1553 vpRxyzVector rxyz(eRc);
1554
1555 os << "Joint Max:" << std::endl
1556 << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t"
1557 << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl
1558
1559 << "Joint Min: " << std::endl
1560 << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t"
1561 << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl
1562
1563 << "Long 5-6: " << std::endl
1564 << "\t" << afma6._long_56 << "\t" << std::endl
1565
1566 << "Coupling 5-6:" << std::endl
1567 << "\t" << afma6._coupl_56 << "\t" << std::endl
1568
1569 << "eMc: " << std::endl
1570 << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t"
1571 << std::endl
1572 << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1573 << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1574 << "\t" << std::endl;
1575
1576 return os;
1577 }
1578