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>
64 static const char *opt_Afma6[] = {
"JOINT_MAX",
"JOINT_MIN",
"LONG_56",
"COUPL_56",
65 "CAMERA",
"eMc_ROT_XYZ",
"eMc_TRANS_XYZ", NULL};
67 #ifdef VISP_HAVE_AFMA6_DATA
69 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_Afma6.cnf");
72 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_ccmop_without_distortion_Afma6.cnf");
75 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_ccmop_with_distortion_Afma6.cnf");
78 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_gripper_without_distortion_Afma6.cnf");
81 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_gripper_with_distortion_Afma6.cnf");
84 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_vacuum_without_distortion_Afma6.cnf");
87 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_vacuum_with_distortion_Afma6.cnf");
90 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_generic_without_distortion_Afma6.cnf");
93 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_generic_with_distortion_Afma6.cnf");
96 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_camera_Afma6.xml");
98 #endif // VISP_HAVE_AFMA6_DATA
115 : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(
vpAfma6::defaultTool),
124 this->_coupl_56 = 0.009091;
126 this->_long_56 = -0.06924;
132 this->_joint_max[0] = 0.7001;
133 this->_joint_max[1] = 0.5201;
134 this->_joint_max[2] = 0.4601;
135 this->_joint_max[3] = 2.7301;
136 this->_joint_max[4] = 2.4801;
137 this->_joint_max[5] = 1.5901;
139 this->_joint_min[0] = -0.6501;
140 this->_joint_min[1] = -0.6001;
141 this->_joint_min[2] = -0.5001;
142 this->_joint_min[3] = -2.7301;
143 this->_joint_min[4] = -0.1001;
144 this->_joint_min[5] = -1.5901;
171 void vpAfma6::init(
const std::string &camera_extrinsic_parameters,
const std::string &camera_intrinsic_parameters)
223 void vpAfma6::init(
const std::string &camera_extrinsic_parameters)
271 #ifdef VISP_HAVE_AFMA6_DATA
273 std::string filename_eMc;
335 #else // VISP_HAVE_AFMA6_DATA
421 #endif // VISP_HAVE_AFMA6_DATA
533 const bool &verbose)
const
536 double q_[2][6], d[2], t;
560 if (fMe[2][2] >= .99999f) {
562 q_[0][4] = q_[1][4] = M_PI / 2.f;
563 t = atan2(fMe[0][0], fMe[0][1]);
564 q_[1][3] = q_[0][3] = q[3];
565 q_[1][5] = q_[0][5] = t - q_[0][3];
577 }
else if (fMe[2][2] <= -.99999) {
579 q_[0][4] = q_[1][4] = -M_PI / 2;
580 t = atan2(fMe[1][1], fMe[1][0]);
581 q_[1][3] = q_[0][3] = q[3];
582 q_[1][5] = q_[0][5] = q_[0][3] - t;
594 q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
596 q_[1][3] = q_[0][3] - M_PI;
598 q_[1][3] = q_[0][3] + M_PI;
600 q_[0][4] = asin(fMe[2][2]);
602 q_[1][4] = M_PI - q_[0][4];
604 q_[1][4] = -M_PI - q_[0][4];
606 q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
608 q_[1][5] = q_[0][5] - M_PI;
610 q_[1][5] = q_[0][5] + M_PI;
612 q_[0][0] = fMe[0][3] - this->
_long_56 * cos(q_[0][3]);
613 q_[1][0] = fMe[0][3] - this->
_long_56 * cos(q_[1][3]);
614 q_[0][1] = fMe[1][3] - this->
_long_56 * sin(q_[0][3]);
615 q_[1][1] = fMe[1][3] - this->
_long_56 * sin(q_[1][3]);
616 q_[0][2] = q_[1][2] = fMe[2][3];
622 for (
int j = 0; j < 2; j++) {
625 for (
unsigned int i = 0; i < 6; i++) {
629 std::cout <<
"Joint " << i <<
" not in limits: " << this->
_joint_min[i] <<
" < " << q_[j][i] <<
" < "
641 std::cout <<
"No solution..." << std::endl;
644 }
else if (ok[1] == 1) {
645 for (
unsigned int i = 0; i < 6; i++)
651 for (
unsigned int i = 0; i < 6; i++)
657 for (
int j = 0; j < 2; j++) {
659 for (
unsigned int i = 3; i < 6; i++)
660 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
662 if (nearest ==
true) {
664 for (
unsigned int i = 0; i < 6; i++)
667 for (
unsigned int i = 0; i < 6; i++)
671 for (
unsigned int i = 0; i < 6; i++)
674 for (
unsigned int i = 0; i < 6; i++)
679 for (
unsigned int i = 0; i < 6; i++)
743 fMc = fMe * this->
_eMc;
774 double q5 = q[5] - this->
_coupl_56 * q[4];
776 double c1 = cos(q[3]);
777 double s1 = sin(q[3]);
778 double c2 = cos(q[4]);
779 double s2 = sin(q[4]);
785 fMe[0][0] = s1 * s2 * c3 + c1 * s3;
786 fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
787 fMe[0][2] = -s1 * c2;
788 fMe[0][3] = q0 + this->
_long_56 * c1;
790 fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
791 fMe[1][1] = c1 * s2 * s3 + s1 * c3;
793 fMe[1][3] = q1 + this->
_long_56 * s1;
796 fMe[2][1] = -c2 * s3;
869 double s4, c4, s5, c5, s6, c6;
879 eJe[0][0] = s4 * s5 * c6 + c4 * s6;
880 eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
882 eJe[0][3] = -this->
_long_56 * s5 * c6;
884 eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
885 eJe[1][1] = c4 * s5 * s6 + s4 * c6;
886 eJe[1][2] = -c5 * s6;
887 eJe[1][3] = this->
_long_56 * s5 * s6;
889 eJe[2][0] = -s4 * c5;
897 eJe[4][3] = -c5 * s6;
940 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
942 double s4 = sin(q[3]);
943 double c4 = cos(q[3]);
949 double s5 = sin(q[4]);
950 double c5 = cos(q[4]);
953 fJe[3][5] = -s4 * c5;
978 for (
unsigned int i = 0; i < 6; i++)
994 for (
unsigned int i = 0; i < 6; i++)
1030 std::ifstream fdconfig(filename.c_str(), std::ios::in);
1032 if (!fdconfig.is_open()) {
1039 bool get_erc =
false;
1040 bool get_etc =
false;
1043 while (std::getline(fdconfig, line)) {
1045 if ((line.compare(0, 1,
"#") == 0) || line.empty()) {
1048 std::istringstream ss(line);
1052 for (code = 0; NULL != opt_Afma6[code]; ++code) {
1053 if (key.compare(opt_Afma6[code]) == 0) {
1081 ss >> erc[0] >> erc[1] >> erc[2];
1084 erc = erc * M_PI / 180.0;
1089 ss >> etc[0] >> etc[1] >> etc[2];
1095 filename.c_str(), lineNum));
1102 if (get_etc && get_erc) {
1191 const unsigned int &image_height)
const
1193 #if defined(VISP_HAVE_XML2) && defined(VISP_HAVE_AFMA6_DATA)
1250 if (image_width == 640 && image_height == 480) {
1262 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1270 if (image_width == 640 && image_height == 480) {
1282 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1290 if (image_width == 640 && image_height == 480) {
1302 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1310 if (image_width == 640 && image_height == 480) {
1322 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1446 os <<
"Joint Max:" << std::endl
1450 <<
"Joint Min: " << std::endl
1454 <<
"Long 5-6: " << std::endl
1455 <<
"\t" << afma6.
_long_56 <<
"\t" << std::endl
1457 <<
"Coupling 5-6:" << std::endl
1458 <<
"\t" << afma6.
_coupl_56 <<
"\t" << std::endl
1460 <<
"eMc: " << std::endl
1461 <<
"\tTranslation (m): " << afma6.
_eMc[0][3] <<
" " << afma6.
_eMc[1][3] <<
" " << afma6.
_eMc[2][3] <<
"\t"
1463 <<
"\tRotation Rxyz (rad) : " << rxyz[0] <<
" " << rxyz[1] <<
" " << rxyz[2] <<
"\t" << std::endl
1465 <<
"\t" << std::endl;