Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpAfma6.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the Irisa's Afma6 robot.
33 *
34*****************************************************************************/
35
44#include <iostream>
45#include <sstream>
46
47#include <visp3/core/vpCameraParameters.h>
48#include <visp3/core/vpDebug.h>
49#include <visp3/core/vpRotationMatrix.h>
50#include <visp3/core/vpRxyzVector.h>
51#include <visp3/core/vpTranslationVector.h>
52#include <visp3/core/vpVelocityTwistMatrix.h>
53#include <visp3/core/vpXmlParserCamera.h>
54#include <visp3/robot/vpAfma6.h>
55#include <visp3/robot/vpRobotException.h>
56
57/* ----------------------------------------------------------------------- */
58/* --- STATIC ------------------------------------------------------------ */
59/* ---------------------------------------------------------------------- */
60
61static const char *opt_Afma6[] = {"JOINT_MAX", "JOINT_MIN", "LONG_56", "COUPL_56",
62 "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL};
63
64#ifdef VISP_HAVE_AFMA6_DATA
65const std::string vpAfma6::CONST_AFMA6_FILENAME =
66 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf");
67
69 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf");
70
72 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf");
73
75 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf");
76
78 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf");
79
81 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf");
82
84 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf");
85
87 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf");
88
90 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf");
91
93 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf");
94
96 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf");
97
99 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml");
100
101#endif // VISP_HAVE_AFMA6_DATA
102
103const char *const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
104const char *const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
105const char *const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
106const char *const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
107const char *const vpAfma6::CONST_INTEL_D435_CAMERA_NAME = "Intel-D435";
108
110
111const unsigned int vpAfma6::njoint = 6;
112
119 : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(vpAfma6::defaultTool),
120 projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
121{
122 // Set the default parameters in case of the config files are not available.
123
124 //
125 // Geometric model constant parameters
126 //
127 // coupling between join 5 and 6
128 this->_coupl_56 = 0.009091;
129 // distance between join 5 and 6
130 this->_long_56 = -0.06924;
131 // Camera extrinsic parameters: effector to camera frame
132 this->_eMc.eye(); // Default values are initialized ...
133 // ... in init (vpAfma6::vpAfma6ToolType tool,
134 // vpCameraParameters::vpCameraParametersProjType projModel)
135 // Maximal value of the joints
136 this->_joint_max[0] = 0.7001;
137 this->_joint_max[1] = 0.5201;
138 this->_joint_max[2] = 0.4601;
139 this->_joint_max[3] = 2.7301;
140 this->_joint_max[4] = 2.4801;
141 this->_joint_max[5] = 1.5901;
142 // Minimal value of the joints
143 this->_joint_min[0] = -0.6501;
144 this->_joint_min[1] = -0.6001;
145 this->_joint_min[2] = -0.5001;
146 this->_joint_min[3] = -2.7301;
147 this->_joint_min[4] = -0.1001;
148 this->_joint_min[5] = -1.5901;
149
150 init();
151}
152
158{
160 return;
161}
162
175void vpAfma6::init(const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters)
176{
177 this->parseConfigFile(camera_extrinsic_parameters);
178
179 this->parseConfigFile(camera_intrinsic_parameters);
180}
181
212void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
213{
214 this->setToolType(tool);
215 this->parseConfigFile(filename.c_str());
216}
217
227void vpAfma6::init(const std::string &camera_extrinsic_parameters)
228{
229 this->parseConfigFile(camera_extrinsic_parameters);
230}
231
248{
249 this->setToolType(tool);
250 this->set_eMc(eMc);
251}
252
271{
272
273 this->projModel = proj_model;
274
275#ifdef VISP_HAVE_AFMA6_DATA
276 // Read the robot parameters from files
277 std::string filename_eMc;
278 switch (tool) {
279 case vpAfma6::TOOL_CCMOP: {
280 switch (projModel) {
283 break;
286 break;
289 "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
290 break;
291 }
292 break;
293 }
295 switch (projModel) {
298 break;
301 break;
304 "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
305 break;
306 }
307 break;
308 }
310 switch (projModel) {
313 break;
316 break;
319 "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;
334 "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
335 break;
336 }
337 break;
338 }
340 switch (projModel) {
343 break;
346 break;
349 "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
350 break;
351 }
352 break;
353 }
354 default: {
355 vpERROR_TRACE("This error should not occur!");
356 break;
357 }
358 }
359
360 this->init(vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
361
362#else // VISP_HAVE_AFMA6_DATA
363
364 // Use here default values of the robot constant parameters.
365 switch (tool) {
366 case vpAfma6::TOOL_CCMOP: {
367 switch (projModel) {
369 _erc[0] = vpMath::rad(164.35); // rx
370 _erc[1] = vpMath::rad(89.64); // ry
371 _erc[2] = vpMath::rad(-73.05); // rz
372 _etc[0] = 0.0117; // tx
373 _etc[1] = 0.0033; // ty
374 _etc[2] = 0.2272; // tz
375 break;
377 _erc[0] = vpMath::rad(33.54); // rx
378 _erc[1] = vpMath::rad(89.34); // ry
379 _erc[2] = vpMath::rad(57.83); // rz
380 _etc[0] = 0.0373; // tx
381 _etc[1] = 0.0024; // ty
382 _etc[2] = 0.2286; // tz
383 break;
386 "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
387 break;
388 }
389 break;
390 }
392 switch (projModel) {
394 _erc[0] = vpMath::rad(88.33); // rx
395 _erc[1] = vpMath::rad(72.07); // ry
396 _erc[2] = vpMath::rad(2.53); // rz
397 _etc[0] = 0.0783; // tx
398 _etc[1] = 0.1234; // ty
399 _etc[2] = 0.1638; // tz
400 break;
402 _erc[0] = vpMath::rad(86.69); // rx
403 _erc[1] = vpMath::rad(71.93); // ry
404 _erc[2] = vpMath::rad(4.17); // rz
405 _etc[0] = 0.1034; // tx
406 _etc[1] = 0.1142; // ty
407 _etc[2] = 0.1642; // tz
408 break;
411 "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
412 break;
413 }
414 break;
415 }
417 switch (projModel) {
419 _erc[0] = vpMath::rad(90.40); // rx
420 _erc[1] = vpMath::rad(75.11); // ry
421 _erc[2] = vpMath::rad(0.18); // rz
422 _etc[0] = 0.0038; // tx
423 _etc[1] = 0.1281; // ty
424 _etc[2] = 0.1658; // tz
425 break;
427 _erc[0] = vpMath::rad(91.61); // rx
428 _erc[1] = vpMath::rad(76.17); // ry
429 _erc[2] = vpMath::rad(-0.98); // rz
430 _etc[0] = 0.0815; // tx
431 _etc[1] = 0.1162; // ty
432 _etc[2] = 0.1658; // tz
433 break;
436 "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
437 break;
438 }
439 break;
440 }
442 switch (projModel) {
444 _erc[0] = vpMath::rad(-71.41); // rx
445 _erc[1] = vpMath::rad(89.49); // ry
446 _erc[2] = vpMath::rad(162.07); // rz
447 _etc[0] = 0.0038; // tx
448 _etc[1] = 0.1281; // ty
449 _etc[2] = 0.1658; // tz
450 break;
452 _erc[0] = vpMath::rad(-52.79); // rx
453 _erc[1] = vpMath::rad(89.55); // ry
454 _erc[2] = vpMath::rad(143.34); // rz
455 _etc[0] = 0.0693; // tx
456 _etc[1] = -0.0297; // ty
457 _etc[2] = 0.1357; // tz
458 break;
461 "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
462 break;
463 }
464 break;
465 }
468 switch (projModel) {
471 // set eMc to identity
472 _erc[0] = 0; // rx
473 _erc[1] = 0; // ry
474 _erc[2] = 0; // rz
475 _etc[0] = 0; // tx
476 _etc[1] = 0; // ty
477 _etc[2] = 0; // tz
478 break;
481 "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
482 break;
483 }
484 break;
485 }
486 }
488 this->_eMc.buildFrom(_etc, eRc);
489#endif // VISP_HAVE_AFMA6_DATA
490
491 setToolType(tool);
492 return;
493}
494
520{
522 fMc = get_fMc(q);
523
524 return fMc;
525}
526
599int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest,
600 const bool &verbose) const
601{
603 double q_[2][6], d[2], t;
604 int ok[2];
605 double cord[6];
606
607 int nbsol = 0;
608
609 if (q.getRows() != njoint)
610 q.resize(6);
611
612 // for(unsigned int i=0;i<3;i++) {
613 // fMe[i][3] = fMc[i][3];
614 // for(int j=0;j<3;j++) {
615 // fMe[i][j] = 0.0;
616 // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
617 // fMe[i][3] -= fMe[i][j]*rpi[j][3];
618 // }
619 // }
620
621 // std::cout << "\n\nfMc: " << fMc;
622 // std::cout << "\n\neMc: " << _eMc;
623
624 fMe = fMc * this->_eMc.inverse();
625 // std::cout << "\n\nfMe: " << fMe;
626
627 if (fMe[2][2] >= .99999f) {
628 vpTRACE("singularity\n");
629 q_[0][4] = q_[1][4] = M_PI / 2.f;
630 t = atan2(fMe[0][0], fMe[0][1]);
631 q_[1][3] = q_[0][3] = q[3];
632 q_[1][5] = q_[0][5] = t - q_[0][3];
633
634 while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
635 /* -> a cause du couplage 4/5 */
636 {
637 q_[1][5] -= vpMath::rad(10);
638 q_[1][3] += vpMath::rad(10);
639 }
640 while (q_[1][5] <= this->_joint_min[5]) {
641 q_[1][5] += vpMath::rad(10);
642 q_[1][3] -= vpMath::rad(10);
643 }
644 } else if (fMe[2][2] <= -.99999) {
645 vpTRACE("singularity\n");
646 q_[0][4] = q_[1][4] = -M_PI / 2;
647 t = atan2(fMe[1][1], fMe[1][0]);
648 q_[1][3] = q_[0][3] = q[3];
649 q_[1][5] = q_[0][5] = q_[0][3] - t;
650 while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
651 /* -> a cause du couplage 4/5 */
652 {
653 q_[1][5] -= vpMath::rad(10);
654 q_[1][3] -= vpMath::rad(10);
655 }
656 while (q_[1][5] <= this->_joint_min[5]) {
657 q_[1][5] += vpMath::rad(10);
658 q_[1][3] += vpMath::rad(10);
659 }
660 } else {
661 q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
662 if (q_[0][3] >= 0.0)
663 q_[1][3] = q_[0][3] - M_PI;
664 else
665 q_[1][3] = q_[0][3] + M_PI;
666
667 q_[0][4] = asin(fMe[2][2]);
668 if (q_[0][4] >= 0.0)
669 q_[1][4] = M_PI - q_[0][4];
670 else
671 q_[1][4] = -M_PI - q_[0][4];
672
673 q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
674 if (q_[0][5] >= 0.0)
675 q_[1][5] = q_[0][5] - M_PI;
676 else
677 q_[1][5] = q_[0][5] + M_PI;
678 }
679 q_[0][0] = fMe[0][3] - this->_long_56 * cos(q_[0][3]);
680 q_[1][0] = fMe[0][3] - this->_long_56 * cos(q_[1][3]);
681 q_[0][1] = fMe[1][3] - this->_long_56 * sin(q_[0][3]);
682 q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]);
683 q_[0][2] = q_[1][2] = fMe[2][3];
684
685 /* prise en compte du couplage axes 5/6 */
686 q_[0][5] += this->_coupl_56 * q_[0][4];
687 q_[1][5] += this->_coupl_56 * q_[1][4];
688
689 for (int j = 0; j < 2; j++) {
690 ok[j] = 1;
691 // test is position is reachable
692 for (unsigned int i = 0; i < 6; i++) {
693 if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
694 if (verbose) {
695 if (i < 3)
696 std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < "
697 << this->_joint_max[i] << std::endl;
698 else
699 std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < "
700 << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
701 }
702 ok[j] = 0;
703 }
704 }
705 }
706 if (ok[0] == 0) {
707 if (ok[1] == 0) {
708 std::cout << "No solution..." << std::endl;
709 nbsol = 0;
710 return nbsol;
711 } else if (ok[1] == 1) {
712 for (unsigned int i = 0; i < 6; i++)
713 cord[i] = q_[1][i];
714 nbsol = 1;
715 }
716 } else {
717 if (ok[1] == 0) {
718 for (unsigned int i = 0; i < 6; i++)
719 cord[i] = q_[0][i];
720 nbsol = 1;
721 } else {
722 nbsol = 2;
723 // vpTRACE("2 solutions\n");
724 for (int j = 0; j < 2; j++) {
725 d[j] = 0.0;
726 for (unsigned int i = 3; i < 6; i++)
727 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
728 }
729 if (nearest == true) {
730 if (d[0] <= d[1])
731 for (unsigned int i = 0; i < 6; i++)
732 cord[i] = q_[0][i];
733 else
734 for (unsigned int i = 0; i < 6; i++)
735 cord[i] = q_[1][i];
736 } else {
737 if (d[0] <= d[1])
738 for (unsigned int i = 0; i < 6; i++)
739 cord[i] = q_[1][i];
740 else
741 for (unsigned int i = 0; i < 6; i++)
742 cord[i] = q_[0][i];
743 }
744 }
745 }
746 for (unsigned int i = 0; i < 6; i++)
747 q[i] = cord[i];
748
749 return nbsol;
750}
751
775{
777 get_fMc(q, fMc);
778
779 return fMc;
780}
781
802{
803
804 // Compute the direct geometric model: fMe = transformation between
805 // fix and end effector frame.
807
808 get_fMe(q, fMe);
809
810 fMc = fMe * this->_eMc;
811
812 return;
813}
814
835{
836 double q0 = q[0]; // meter
837 double q1 = q[1]; // meter
838 double q2 = q[2]; // meter
839
840 /* Decouplage liaisons 2 et 3. */
841 double q5 = q[5] - this->_coupl_56 * q[4];
842
843 double c1 = cos(q[3]);
844 double s1 = sin(q[3]);
845 double c2 = cos(q[4]);
846 double s2 = sin(q[4]);
847 double c3 = cos(q5);
848 double s3 = sin(q5);
849
850 // Compute the direct geometric model: fMe = transformation betwee
851 // fix and end effector frame.
852 fMe[0][0] = s1 * s2 * c3 + c1 * s3;
853 fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
854 fMe[0][2] = -s1 * c2;
855 fMe[0][3] = q0 + this->_long_56 * c1;
856
857 fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
858 fMe[1][1] = c1 * s2 * s3 + s1 * c3;
859 fMe[1][2] = c1 * c2;
860 fMe[1][3] = q1 + this->_long_56 * s1;
861
862 fMe[2][0] = c2 * c3;
863 fMe[2][1] = -c2 * s3;
864 fMe[2][2] = s2;
865 fMe[2][3] = q2;
866
867 fMe[3][0] = 0;
868 fMe[3][1] = 0;
869 fMe[3][2] = 0;
870 fMe[3][3] = 1;
871
872 // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
873
874 return;
875}
876
887void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
898vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
899
910{
912 get_cMe(cMe);
913
914 cVe.buildFrom(cMe);
915
916 return;
917}
918
931void vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
932{
933
934 eJe.resize(6, 6);
935
936 double s4, c4, s5, c5, s6, c6;
937
938 s4 = sin(q[3]);
939 c4 = cos(q[3]);
940 s5 = sin(q[4]);
941 c5 = cos(q[4]);
942 s6 = sin(q[5] - this->_coupl_56 * q[4]);
943 c6 = cos(q[5] - this->_coupl_56 * q[4]);
944
945 eJe = 0;
946 eJe[0][0] = s4 * s5 * c6 + c4 * s6;
947 eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
948 eJe[0][2] = c5 * c6;
949 eJe[0][3] = -this->_long_56 * s5 * c6;
950
951 eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
952 eJe[1][1] = c4 * s5 * s6 + s4 * c6;
953 eJe[1][2] = -c5 * s6;
954 eJe[1][3] = this->_long_56 * s5 * s6;
955
956 eJe[2][0] = -s4 * c5;
957 eJe[2][1] = c4 * c5;
958 eJe[2][2] = s5;
959 eJe[2][3] = this->_long_56 * c5;
960
961 eJe[3][3] = c5 * c6;
962 eJe[3][4] = s6;
963
964 eJe[4][3] = -c5 * s6;
965 eJe[4][4] = c6;
966
967 eJe[5][3] = s5;
968 eJe[5][4] = -this->_coupl_56;
969 eJe[5][5] = 1;
970
971 return;
972}
973
1001void vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1002{
1003
1004 fJe.resize(6, 6);
1005
1006 // block superieur gauche
1007 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1008
1009 double s4 = sin(q[3]);
1010 double c4 = cos(q[3]);
1011
1012 // block superieur droit
1013 fJe[0][3] = -this->_long_56 * s4;
1014 fJe[1][3] = this->_long_56 * c4;
1015
1016 double s5 = sin(q[4]);
1017 double c5 = cos(q[4]);
1018 // block inferieur droit
1019 fJe[3][4] = c4;
1020 fJe[3][5] = -s4 * c5;
1021 fJe[4][4] = s4;
1022 fJe[4][5] = c4 * c5;
1023 fJe[5][3] = 1;
1024 fJe[5][5] = s5;
1025
1026 // coupling between joint 5 and 6
1027 fJe[3][4] += this->_coupl_56 * s4 * c5;
1028 fJe[4][4] += -this->_coupl_56 * c4 * c5;
1029 fJe[5][4] += -this->_coupl_56 * s5;
1030
1031 return;
1032}
1033
1043{
1044 vpColVector qmin(6);
1045 for (unsigned int i = 0; i < 6; i++)
1046 qmin[i] = this->_joint_min[i];
1047 return qmin;
1048}
1049
1059{
1060 vpColVector qmax(6);
1061 for (unsigned int i = 0; i < 6; i++)
1062 qmax[i] = this->_joint_max[i];
1063 return qmax;
1064}
1065
1072double vpAfma6::getCoupl56() const { return _coupl_56; }
1073
1080double vpAfma6::getLong56() const { return _long_56; }
1081
1092void vpAfma6::parseConfigFile(const std::string &filename)
1093{
1094 vpRxyzVector erc; // eMc rotation
1095 vpTranslationVector etc; // eMc translation
1096
1097 std::ifstream fdconfig(filename.c_str(), std::ios::in);
1098
1099 if (!fdconfig.is_open()) {
1100 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
1101 filename.c_str());
1102 }
1103
1104 std::string line;
1105 int lineNum = 0;
1106 bool get_erc = false;
1107 bool get_etc = false;
1108 int code;
1109
1110 while (std::getline(fdconfig, line)) {
1111 lineNum++;
1112 if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
1113 continue;
1114 }
1115 std::istringstream ss(line);
1116 std::string key;
1117 ss >> key;
1118
1119 for (code = 0; NULL != opt_Afma6[code]; ++code) {
1120 if (key.compare(opt_Afma6[code]) == 0) {
1121 break;
1122 }
1123 }
1124
1125 switch (code) {
1126 case 0:
1127 ss >> this->_joint_max[0] >> this->_joint_max[1] >> this->_joint_max[2] >> this->_joint_max[3] >>
1128 this->_joint_max[4] >> this->_joint_max[5];
1129 break;
1130
1131 case 1:
1132 ss >> this->_joint_min[0] >> this->_joint_min[1] >> this->_joint_min[2] >> this->_joint_min[3] >>
1133 this->_joint_min[4] >> this->_joint_min[5];
1134 break;
1135
1136 case 2:
1137 ss >> this->_long_56;
1138 break;
1139
1140 case 3:
1141 ss >> this->_coupl_56;
1142 break;
1143
1144 case 4:
1145 break; // Nothing to do: camera name
1146
1147 case 5:
1148 ss >> erc[0] >> erc[1] >> erc[2];
1149
1150 // Convert rotation from degrees to radians
1151 erc = erc * M_PI / 180.0;
1152 get_erc = true;
1153 break;
1154
1155 case 6:
1156 ss >> etc[0] >> etc[1] >> etc[2];
1157 get_etc = true;
1158 break;
1159
1160 default:
1161 throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
1162 filename.c_str(), lineNum));
1163 }
1164 }
1165
1166 fdconfig.close();
1167
1168 // Compute the eMc matrix from the translations and rotations
1169 if (get_etc && get_erc) {
1170 _erc = erc;
1171 _etc = etc;
1172
1174 this->_eMc.buildFrom(_etc, eRc);
1175 }
1176}
1177
1187{
1188 this->_eMc = eMc;
1189 this->_etc = eMc.getTranslationVector();
1190
1191 vpRotationMatrix R(eMc);
1192 this->_erc.buildFrom(R);
1193}
1194
1255void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
1256 const unsigned int &image_height) const
1257{
1258#if defined(VISP_HAVE_AFMA6_DATA)
1259 vpXmlParserCamera parser;
1260 switch (getToolType()) {
1261 case vpAfma6::TOOL_CCMOP: {
1262 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1263 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1265 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1266 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1267 }
1268 break;
1269 }
1270 case vpAfma6::TOOL_GRIPPER: {
1271 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1272 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1274 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1275 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1276 }
1277 break;
1278 }
1279 case vpAfma6::TOOL_VACUUM: {
1280 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1281 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1283 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1284 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1285 }
1286 break;
1287 }
1289 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\"" << std::endl
1290 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1292 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1293 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1294 }
1295 break;
1296 }
1298 std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1299 << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1301 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1302 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1303 }
1304 break;
1305 }
1306 default: {
1307 vpERROR_TRACE("This error should not occur!");
1308 // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1309 // "que les specs de la classe ont ete modifiee, "
1310 // "et que le code n'a pas ete mis a jour "
1311 // "correctement.");
1312 // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1313 // "vpAfma6::vpAfma6ToolType, et controlez que "
1314 // "tous les cas ont ete pris en compte dans la "
1315 // "fonction init(camera).");
1316 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1317 }
1318 }
1319#else
1320 // Set default parameters
1321 switch (getToolType()) {
1322 case vpAfma6::TOOL_CCMOP: {
1323 // Set default intrinsic camera parameters for 640x480 images
1324 if (image_width == 640 && image_height == 480) {
1325 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
1326 << std::endl;
1327 switch (this->projModel) {
1329 cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1330 break;
1332 cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1333 break;
1336 "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;
1360 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1361 break;
1362 }
1363 } else {
1364 vpTRACE("Cannot get default intrinsic camera parameters for this image "
1365 "resolution");
1366 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1367 }
1368 break;
1369 }
1370 case vpAfma6::TOOL_VACUUM: {
1371 // Set default intrinsic camera parameters for 640x480 images
1372 if (image_width == 640 && image_height == 480) {
1373 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\""
1374 << std::endl;
1375 switch (this->projModel) {
1377 cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1378 break;
1380 cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1381 break;
1384 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1385 break;
1386 }
1387 } else {
1388 vpTRACE("Cannot get default intrinsic camera parameters for this image "
1389 "resolution");
1390 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1391 }
1392 break;
1393 }
1395 // Set default intrinsic camera parameters for 640x480 images
1396 if (image_width == 640 && image_height == 480) {
1397 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\""
1398 << std::endl;
1399 switch (this->projModel) {
1401 cam.initPersProjWithoutDistortion(605.4, 605.6, 328.6, 241.0);
1402 break;
1404 cam.initPersProjWithDistortion(611.8, 612.6, 327.8, 241.7, 0.0436, -0.04265);
1405 break;
1408 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1409 break;
1410 }
1411 } else {
1412 vpTRACE("Cannot get default intrinsic camera parameters for this image "
1413 "resolution");
1414 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1415 }
1416 break;
1417 }
1419 // Set default intrinsic camera parameters for 640x480 images
1420 if (image_width == 640 && image_height == 480) {
1421 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\""
1422 << std::endl;
1423 switch (this->projModel) {
1425 cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1426 break;
1428 cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1429 break;
1432 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1433 break;
1434 }
1435 } else {
1436 vpTRACE("Cannot get default intrinsic camera parameters for this image "
1437 "resolution");
1438 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1439 }
1440 break;
1441 }
1442 default:
1443 vpERROR_TRACE("This error should not occur!");
1444 break;
1445 }
1446#endif
1447 return;
1448}
1449
1543
1553VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6)
1554{
1555 vpRotationMatrix eRc;
1556 afma6._eMc.extract(eRc);
1557 vpRxyzVector rxyz(eRc);
1558
1559 os << "Joint Max:" << std::endl
1560 << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t"
1561 << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl
1562
1563 << "Joint Min: " << std::endl
1564 << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t"
1565 << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl
1566
1567 << "Long 5-6: " << std::endl
1568 << "\t" << afma6._long_56 << "\t" << std::endl
1569
1570 << "Coupling 5-6:" << std::endl
1571 << "\t" << afma6._coupl_56 << "\t" << std::endl
1572
1573 << "eMc: " << std::endl
1574 << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t"
1575 << std::endl
1576 << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1577 << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1578 << "\t" << std::endl;
1579
1580 return os;
1581}
Modelisation of Irisa's gantry robot named Afma6.
Definition vpAfma6.h:76
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition vpAfma6.h:91
static const char *const CONST_CCMOP_CAMERA_NAME
Definition vpAfma6.h:99
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
Definition vpAfma6.h:84
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition vpAfma6.cpp:1186
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition vpAfma6.h:83
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition vpAfma6.cpp:909
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
Definition vpAfma6.h:89
vpColVector getJointMax() const
Definition vpAfma6.cpp:1058
static const std::string CONST_CAMERA_AFMA6_FILENAME
Definition vpAfma6.h:93
double _joint_min[6]
Definition vpAfma6.h:201
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition vpAfma6.cpp:519
static const unsigned int njoint
Number of joint.
Definition vpAfma6.h:195
void init(void)
Definition vpAfma6.cpp:157
vpRxyzVector _erc
Definition vpAfma6.h:204
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition vpAfma6.cpp:599
static const char *const CONST_VACUUM_CAMERA_NAME
Definition vpAfma6.h:109
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition vpAfma6.cpp:834
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition vpAfma6.h:85
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition vpAfma6.cpp:887
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition vpAfma6.h:191
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition vpAfma6.h:166
double _coupl_56
Definition vpAfma6.h:198
static const std::string CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME
Definition vpAfma6.h:90
vpColVector getJointMin() const
Definition vpAfma6.cpp:1042
static const char *const CONST_INTEL_D435_CAMERA_NAME
Definition vpAfma6.h:120
double getCoupl56() const
Definition vpAfma6.cpp:1072
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition vpAfma6.h:133
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition vpAfma6.h:104
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
Definition vpAfma6.h:86
vpHomogeneousMatrix _eMc
Definition vpAfma6.h:206
double _long_56
Definition vpAfma6.h:199
void parseConfigFile(const std::string &filename)
Definition vpAfma6.cpp:1092
vpTranslationVector _etc
Definition vpAfma6.h:203
vpHomogeneousMatrix get_eMc() const
Definition vpAfma6.cpp:898
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition vpAfma6.cpp:1255
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition vpAfma6.cpp:774
static const char *const CONST_GENERIC_CAMERA_NAME
Definition vpAfma6.h:114
vpCameraParameters::vpCameraParametersProjType projModel
Definition vpAfma6.h:212
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
Definition vpAfma6.h:87
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition vpAfma6.h:92
double _joint_max[6]
Definition vpAfma6.h:200
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
Definition vpAfma6.h:88
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition vpAfma6.cpp:931
static const std::string CONST_AFMA6_FILENAME
Definition vpAfma6.h:82
double getLong56() const
Definition vpAfma6.cpp:1080
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition vpAfma6.cpp:1001
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition vpAfma6.h:123
@ TOOL_CCMOP
Definition vpAfma6.h:124
@ TOOL_GENERIC_CAMERA
Definition vpAfma6.h:127
@ TOOL_CUSTOM
Definition vpAfma6.h:129
@ TOOL_VACUUM
Definition vpAfma6.h:126
@ TOOL_INTEL_D435_CAMERA
Definition vpAfma6.h:128
@ TOOL_GRIPPER
Definition vpAfma6.h:125
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ notImplementedError
Not implemented.
Definition vpException.h:81
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
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Error that can be emitted by the vpRobot class and its derivatives.
@ readingParametersError
Cannot parse parameters.
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
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, bool verbose=true)
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388