51 #include <visp/vpDebug.h>
52 #include <visp/vpVelocityTwistMatrix.h>
53 #include <visp/vpRobotException.h>
54 #include <visp/vpXmlParserCamera.h>
55 #include <visp/vpCameraParameters.h>
56 #include <visp/vpRxyzVector.h>
57 #include <visp/vpTranslationVector.h>
58 #include <visp/vpRotationMatrix.h>
59 #include <visp/vpAfma6.h>
65 #ifdef VISP_HAVE_ACCESS_TO_NAS
66 static const char *opt_Afma6[] = {
"JOINT_MAX",
"JOINT_MIN",
"LONG_56",
"COUPL_56",
67 "CAMERA",
"eMc_ROT_XYZ",
"eMc_TRANS_XYZ",
72 =
"Z:/robot/Afma6/current/include/const_Afma6.cnf";
74 =
"/udd/fspindle/robot/Afma6/current/include/const_Afma6.cnf";
79 =
"Z:/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf";
81 =
"/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf";
86 =
"Z:/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf";
88 =
"/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf";
93 =
"Z:/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf";
95 =
"/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf";
100 =
"Z:/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf";
102 =
"/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf";
107 =
"Z:/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf";
109 =
"/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf";
114 =
"Z:/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf";
116 =
"/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf";
121 =
"Z:/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf";
123 =
"/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf";
128 =
"Z:/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf";
130 =
"/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf";
135 =
"Z:/robot/Afma6/current/include/const_camera_Afma6.xml";
137 =
"/udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml";
140 #endif // VISP_HAVE_ACCESS_TO_NAS
216 #ifdef VISP_HAVE_ACCESS_TO_NAS
219 const char * paramCamera)
250 #ifdef VISP_HAVE_ACCESS_TO_NAS
252 char filename_eMc [FILENAME_MAX];
258 snprintf(filename_eMc, FILENAME_MAX,
"%s",
261 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
267 snprintf(filename_eMc, FILENAME_MAX,
"%s",
270 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
281 snprintf(filename_eMc, FILENAME_MAX,
"%s",
284 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
290 snprintf(filename_eMc, FILENAME_MAX,
"%s",
293 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
304 snprintf(filename_eMc, FILENAME_MAX,
"%s",
307 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
313 snprintf(filename_eMc, FILENAME_MAX,
"%s",
316 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
327 snprintf(filename_eMc, FILENAME_MAX,
"%s",
330 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
336 snprintf(filename_eMc, FILENAME_MAX,
"%s",
339 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
362 #else // VISP_HAVE_ACCESS_TO_NAS
443 #endif // VISP_HAVE_ACCESS_TO_NAS
559 double q_[2][6],d[2],t;
584 if (fMe[2][2] >= .99999f)
587 q_[0][4] = q_[1][4] = M_PI/2.f;
588 t = atan2(fMe[0][0],fMe[0][1]);
589 q_[1][3] = q_[0][3] = q[3];
590 q_[1][5] = q_[0][5] = t - q_[0][3];
604 else if (fMe[2][2] <= -.99999)
607 q_[0][4] = q_[1][4] = -M_PI/2;
608 t = atan2(fMe[1][1],fMe[1][0]);
609 q_[1][3] = q_[0][3] = q[3];
610 q_[1][5] = q_[0][5] = q_[0][3] - t;
625 q_[0][3] = atan2(-fMe[0][2],fMe[1][2]);
626 if (q_[0][3] >= 0.0) q_[1][3] = q_[0][3] - M_PI;
627 else q_[1][3] = q_[0][3] + M_PI;
629 q_[0][4] = asin(fMe[2][2]);
630 if (q_[0][4] >= 0.0) q_[1][4] = M_PI - q_[0][4];
631 else q_[1][4] = -M_PI - q_[0][4];
633 q_[0][5] = atan2(-fMe[2][1],fMe[2][0]);
634 if (q_[0][5] >= 0.0) q_[1][5] = q_[0][5] - M_PI;
635 else q_[1][5] = q_[0][5] + M_PI;
637 q_[0][0] = fMe[0][3] - this->
_long_56*cos(q_[0][3]);
638 q_[1][0] = fMe[0][3] - this->
_long_56*cos(q_[1][3]);
639 q_[0][1] = fMe[1][3] - this->
_long_56*sin(q_[0][3]);
640 q_[1][1] = fMe[1][3] - this->
_long_56*sin(q_[1][3]);
641 q_[0][2] = q_[1][2] = fMe[2][3];
647 for (
int j=0;j<2;j++)
651 for (
unsigned int i=0;i<6;i++) {
659 std::cout <<
"No solution..." << std::endl;
663 else if (ok[1] == 1) {
664 for (
unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
671 for (
unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
678 for (
int j=0;j<2;j++)
681 for (
unsigned int i=3;i<6;i++)
682 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
687 for (
unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
689 for (
unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
694 for (
unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
696 for (
unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
700 for(
unsigned int i=0; i<6; i++)
766 fMc = fMe * this->
_eMc;
798 double q5 = q[5] - this->
_coupl_56 * q[4];
800 double c1 = cos(q[3]);
801 double s1 = sin(q[3]);
802 double c2 = cos(q[4]);
803 double s2 = sin(q[4]);
809 fMe[0][0] = s1*s2*c3 + c1*s3;
810 fMe[0][1] = -s1*s2*s3 + c1*c3;
814 fMe[1][0] = -c1*s2*c3 + s1*s3;
815 fMe[1][1] = c1*s2*s3 + s1*c3;
888 double s3,c3,s4,c4,s5,c5 ;
890 s3=sin(q[3]); c3=cos(q[3]);
891 s4=sin(q[4]); c4=cos(q[4]);
892 s5=sin(q[5]); c5=cos(q[5]);
895 eJe[0][0] = s3*s4*c5+c3*s5;
896 eJe[0][1] = -c3*s4*c5+s3*s5;
900 eJe[1][0] = -s3*s4*s5+c3*c5;
901 eJe[1][1] = c3*s4*s5+s3*c5;
956 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1 ;
958 double s4 = sin(q[4]) ;
959 double c4 = cos(q[4]) ;
967 double s5 = sin(q[5]) ;
968 double c5 = cos(q[5]) ;
970 fJe[3][4] = c4 ; fJe[3][5] = -s4*c5 ;
971 fJe[4][4] = s4 ; fJe[4][5] = c4*c5 ;
972 fJe[5][3] = 1 ; fJe[5][5] = s5 ;
990 for (
unsigned int i=0; i < 6; i ++)
1007 for (
unsigned int i=0; i < 6; i ++)
1049 #ifdef VISP_HAVE_ACCESS_TO_NAS
1055 char Ligne[FILENAME_MAX];
1056 char namoption[100];
1060 double trans_eMc[3];
1061 bool get_rot_eMc =
false;
1062 bool get_trans_eMc =
false;
1065 if ((fdtask = fopen(filename,
"r" )) == NULL)
1070 "Impossible to read the config file.");
1073 while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
1075 if (
'#' == Ligne[0]) {
continue; }
1076 sscanf(Ligne,
"%s", namoption);
1077 dim = strlen(namoption);
1080 NULL != opt_Afma6[code];
1083 if (strncmp(opt_Afma6[code], namoption, dim) == 0)
1092 sscanf(Ligne,
"%s %lf %lf %lf %lf %lf %lf",
1100 sscanf(Ligne,
"%s %lf %lf %lf %lf %lf %lf", namoption,
1107 sscanf(Ligne,
"%s %lf", namoption, &this->
_long_56);
1111 sscanf(Ligne,
"%s %lf", namoption, &this->
_coupl_56);
1118 sscanf(Ligne,
"%s %lf %lf %lf", namoption,
1124 rot_eMc[0] *= M_PI / 180.0;
1125 rot_eMc[1] *= M_PI / 180.0;
1126 rot_eMc[2] *= M_PI / 180.0;
1131 sscanf(Ligne,
"%s %lf %lf %lf", namoption,
1135 get_trans_eMc =
true;
1140 "ligne #%d.", filename, numLn);
1147 if (get_rot_eMc && get_trans_eMc) {
1148 for (
unsigned int i=0; i < 3; i ++) {
1149 _erc[i] = rot_eMc[i];
1150 _etc[i] = trans_eMc[i];
1226 const unsigned int &image_width,
1227 const unsigned int &image_height)
1229 #if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
1233 std::cout <<
"Get camera parameters for camera \""
1235 <<
"from the XML file: \""
1237 if (parser.
parse(cam,
1243 "Impossible to read the camera parameters.");
1248 std::cout <<
"Get camera parameters for camera \""
1250 <<
"from the XML file: \""
1252 if (parser.
parse(cam,
1258 "Impossible to read the camera parameters.");
1263 std::cout <<
"Get camera parameters for camera \""
1265 <<
"from the XML file: \""
1267 if (parser.
parse(cam,
1273 "Impossible to read the camera parameters.");
1278 std::cout <<
"Get camera parameters for camera \""
1280 <<
"from the XML file: \""
1282 if (parser.
parse(cam,
1288 "Impossible to read the camera parameters.");
1303 "Impossible to read the camera parameters.");
1312 if (image_width == 640 && image_height == 480) {
1313 std::cout <<
"Get default camera parameters for camera \""
1325 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1327 "Impossible to read the camera parameters.");
1333 if (image_width == 640 && image_height == 480) {
1334 std::cout <<
"Get default camera parameters for camera \""
1346 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1348 "Impossible to read the camera parameters.");
1354 if (image_width == 640 && image_height == 480) {
1355 std::cout <<
"Get default camera parameters for camera \""
1367 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1369 "Impossible to read the camera parameters.");
1375 if (image_width == 640 && image_height == 480) {
1376 std::cout <<
"Get default camera parameters for camera \""
1388 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1390 "Impossible to read the camera parameters.");
1517 <<
"Joint Max:" << std::endl
1524 <<
"\t" << std::endl
1526 <<
"Joint Min: " << std::endl
1533 <<
"\t" << std::endl
1535 <<
"Long 5-6: " << std::endl
1537 <<
"\t" << std::endl
1539 <<
"Coupling 5-6:" << std::endl
1541 <<
"\t" << std::endl
1543 <<
"eMc: "<< std::endl
1544 <<
"\tTranslation (m): "
1545 << afma6.
_eMc[0][3] <<
" "
1546 << afma6.
_eMc[1][3] <<
" "
1548 <<
"\t" << std::endl
1549 <<
"\tRotation Rxyz (rad) : "
1553 <<
"\t" << std::endl
1554 <<
"\tRotation Rxyz (deg) : "
1558 <<
"\t" << std::endl;