Visual Servoing Platform version 3.5.0
vpAfma6.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 Irisa's Afma6 robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
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
64static 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
68const std::string vpAfma6::CONST_AFMA6_FILENAME =
69 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf");
70
72 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf");
73
75 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf");
76
78 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf");
79
81 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf");
82
84 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf");
85
87 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf");
88
90 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf");
91
93 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf");
94
96 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf");
97
99 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf");
100
101const 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
106const char *const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
107const char *const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
108const char *const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
109const char *const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
110const char *const vpAfma6::CONST_INTEL_D435_CAMERA_NAME = "Intel-D435";
111
113
114const unsigned int vpAfma6::njoint = 6;
115
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
161{
163 return;
164}
165
178void 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
215void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
216{
217 this->setToolType(tool);
218 this->parseConfigFile(filename.c_str());
219}
220
230void vpAfma6::init(const std::string &camera_extrinsic_parameters)
231{
232 this->parseConfigFile(camera_extrinsic_parameters);
233}
234
251{
252 this->setToolType(tool);
253 this->set_eMc(eMc);
254}
255
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) {
286 break;
289 break;
291 throw vpException(vpException::notImplementedError, "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
292 break;
293 }
294 break;
295 }
297 switch (projModel) {
300 break;
303 break;
305 throw vpException(vpException::notImplementedError, "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
306 break;
307 }
308 break;
309 }
311 switch (projModel) {
314 break;
317 break;
319 throw vpException(vpException::notImplementedError, "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
320 break;
321 }
322 break;
323 }
325 switch (projModel) {
328 break;
331 break;
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 }
339 switch (projModel) {
342 break;
345 break;
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) {
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;
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;
391 throw vpException(vpException::notImplementedError, "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
392 break;
393 }
394 break;
395 }
397 switch (projModel) {
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;
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;
415 throw vpException(vpException::notImplementedError, "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
416 break;
417 }
418 break;
419 }
421 switch (projModel) {
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;
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;
439 throw vpException(vpException::notImplementedError, "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
440 break;
441 }
442 break;
443 }
445 switch (projModel) {
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;
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;
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 }
470 switch (projModel) {
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;
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 }
489 this->_eMc.buildFrom(_etc, eRc);
490#endif // VISP_HAVE_AFMA6_DATA
491
492 setToolType(tool);
493 return;
494}
495
521{
523 fMc = get_fMc(q);
524
525 return fMc;
526}
527
600int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest,
601 const bool &verbose) const
602{
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
776{
778 get_fMc(q, fMc);
779
780 return fMc;
781}
782
803{
804
805 // Compute the direct geometric model: fMe = transformation between
806 // fix and end effector frame.
808
809 get_fMe(q, fMe);
810
811 fMc = fMe * this->_eMc;
812
813 return;
814}
815
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
888void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
899vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
900
911{
913 get_cMe(cMe);
914
915 cVe.buildFrom(cMe);
916
917 return;
918}
919
932void 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
1002void 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
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
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
1073double vpAfma6::getCoupl56() const { return _coupl_56; }
1074
1081double vpAfma6::getLong56() const { return _long_56; }
1082
1093void 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
1175 this->_eMc.buildFrom(_etc, eRc);
1176 }
1177}
1178
1188{
1189 this->_eMc = eMc;
1190 this->_etc = eMc.getTranslationVector();
1191
1192 vpRotationMatrix R(eMc);
1193 this->_erc.buildFrom(R);
1194}
1195
1256void 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;
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;
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;
1284 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1285 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1286 }
1287 break;
1288 }
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;
1293 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1294 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1295 }
1296 break;
1297 }
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;
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) {
1330 cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1331 break;
1333 cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1334 break;
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) {
1353 cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
1354 break;
1356 cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
1357 break;
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) {
1376 cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1377 break;
1379 cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1380 break;
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 }
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) {
1399 cam.initPersProjWithoutDistortion(605.4, 605.6, 328.6, 241.0);
1400 break;
1402 cam.initPersProjWithDistortion(611.8, 612.6, 327.8, 241.7, 0.0436, -0.04265);
1403 break;
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 }
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) {
1422 cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1423 break;
1425 cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1426 break;
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
1489{
1490 getCameraParameters(cam, I.getWidth(), I.getHeight());
1491}
1536{
1537 getCameraParameters(cam, I.getWidth(), I.getHeight());
1538}
1539
1549VISP_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}
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:79
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:94
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:102
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:87
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1187
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:86
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:910
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:92
vpColVector getJointMax() const
Definition: vpAfma6.cpp:1059
static const std::string CONST_CAMERA_AFMA6_FILENAME
Definition: vpAfma6.h:96
double _joint_min[6]
Definition: vpAfma6.h:204
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma6.cpp:520
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:198
void init(void)
Definition: vpAfma6.cpp:160
vpRxyzVector _erc
Definition: vpAfma6.h:207
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:600
static const char *const CONST_VACUUM_CAMERA_NAME
Definition: vpAfma6.h:112
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma6.cpp:835
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:88
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:888
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:194
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:169
double _coupl_56
Definition: vpAfma6.h:201
static const std::string CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:93
vpColVector getJointMin() const
Definition: vpAfma6.cpp:1043
static const char *const CONST_INTEL_D435_CAMERA_NAME
Definition: vpAfma6.h:123
double getCoupl56() const
Definition: vpAfma6.cpp:1073
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:136
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:107
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:89
vpAfma6()
Definition: vpAfma6.cpp:121
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:209
double _long_56
Definition: vpAfma6.h:202
void parseConfigFile(const std::string &filename)
Definition: vpAfma6.cpp:1093
vpTranslationVector _etc
Definition: vpAfma6.h:206
vpHomogeneousMatrix get_eMc() const
Definition: vpAfma6.cpp:899
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1256
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:775
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpAfma6.h:117
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:215
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:90
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:95
double _joint_max[6]
Definition: vpAfma6.h:203
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:91
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:932
static const std::string CONST_AFMA6_FILENAME
Definition: vpAfma6.h:85
double getLong56() const
Definition: vpAfma6.cpp:1081
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1002
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:126
@ TOOL_CCMOP
Definition: vpAfma6.h:127
@ TOOL_GENERIC_CAMERA
Definition: vpAfma6.h:130
@ TOOL_CUSTOM
Definition: vpAfma6.h:132
@ TOOL_VACUUM
Definition: vpAfma6.h:129
@ TOOL_INTEL_D435_CAMERA
Definition: vpAfma6.h:131
@ TOOL_GRIPPER
Definition: vpAfma6.h:128
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
unsigned int getRows() const
Definition: vpArray2D.h:289
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)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ notImplementedError
Not implemented.
Definition: vpException.h:93
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
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
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
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
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
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