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
558 vpColVector & q,
const bool &nearest,
const bool &verbose)
561 double q_[2][6],d[2],t;
586 if (fMe[2][2] >= .99999f)
589 q_[0][4] = q_[1][4] = M_PI/2.f;
590 t = atan2(fMe[0][0],fMe[0][1]);
591 q_[1][3] = q_[0][3] = q[3];
592 q_[1][5] = q_[0][5] = t - q_[0][3];
606 else if (fMe[2][2] <= -.99999)
609 q_[0][4] = q_[1][4] = -M_PI/2;
610 t = atan2(fMe[1][1],fMe[1][0]);
611 q_[1][3] = q_[0][3] = q[3];
612 q_[1][5] = q_[0][5] = q_[0][3] - t;
627 q_[0][3] = atan2(-fMe[0][2],fMe[1][2]);
628 if (q_[0][3] >= 0.0) q_[1][3] = q_[0][3] - M_PI;
629 else q_[1][3] = q_[0][3] + M_PI;
631 q_[0][4] = asin(fMe[2][2]);
632 if (q_[0][4] >= 0.0) q_[1][4] = M_PI - q_[0][4];
633 else q_[1][4] = -M_PI - q_[0][4];
635 q_[0][5] = atan2(-fMe[2][1],fMe[2][0]);
636 if (q_[0][5] >= 0.0) q_[1][5] = q_[0][5] - M_PI;
637 else q_[1][5] = q_[0][5] + M_PI;
639 q_[0][0] = fMe[0][3] - this->
_long_56*cos(q_[0][3]);
640 q_[1][0] = fMe[0][3] - this->
_long_56*cos(q_[1][3]);
641 q_[0][1] = fMe[1][3] - this->
_long_56*sin(q_[0][3]);
642 q_[1][1] = fMe[1][3] - this->
_long_56*sin(q_[1][3]);
643 q_[0][2] = q_[1][2] = fMe[2][3];
649 for (
int j=0;j<2;j++)
653 for (
unsigned int i=0;i<6;i++) {
657 std::cout <<
"Joint " << i <<
" not in limits: " << this->
_joint_min[i] <<
" < " << q_[j][i] <<
" < " << this->
_joint_max[i] << std::endl;
668 std::cout <<
"No solution..." << std::endl;
672 else if (ok[1] == 1) {
673 for (
unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
680 for (
unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
687 for (
int j=0;j<2;j++)
690 for (
unsigned int i=3;i<6;i++)
691 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
696 for (
unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
698 for (
unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
703 for (
unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
705 for (
unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
709 for(
unsigned int i=0; i<6; i++)
775 fMc = fMe * this->
_eMc;
807 double q5 = q[5] - this->
_coupl_56 * q[4];
809 double c1 = cos(q[3]);
810 double s1 = sin(q[3]);
811 double c2 = cos(q[4]);
812 double s2 = sin(q[4]);
818 fMe[0][0] = s1*s2*c3 + c1*s3;
819 fMe[0][1] = -s1*s2*s3 + c1*c3;
823 fMe[1][0] = -c1*s2*c3 + s1*s3;
824 fMe[1][1] = c1*s2*s3 + s1*c3;
897 double s3,c3,s4,c4,s5,c5 ;
899 s3=sin(q[3]); c3=cos(q[3]);
900 s4=sin(q[4]); c4=cos(q[4]);
901 s5=sin(q[5]); c5=cos(q[5]);
904 eJe[0][0] = s3*s4*c5+c3*s5;
905 eJe[0][1] = -c3*s4*c5+s3*s5;
909 eJe[1][0] = -s3*s4*s5+c3*c5;
910 eJe[1][1] = c3*s4*s5+s3*c5;
965 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1 ;
967 double s4 = sin(q[4]) ;
968 double c4 = cos(q[4]) ;
976 double s5 = sin(q[5]) ;
977 double c5 = cos(q[5]) ;
979 fJe[3][4] = c4 ; fJe[3][5] = -s4*c5 ;
980 fJe[4][4] = s4 ; fJe[4][5] = c4*c5 ;
981 fJe[5][3] = 1 ; fJe[5][5] = s5 ;
999 for (
unsigned int i=0; i < 6; i ++)
1016 for (
unsigned int i=0; i < 6; i ++)
1058 #ifdef VISP_HAVE_ACCESS_TO_NAS
1064 char Ligne[FILENAME_MAX];
1065 char namoption[100];
1069 double trans_eMc[3];
1070 bool get_rot_eMc =
false;
1071 bool get_trans_eMc =
false;
1074 if ((fdtask = fopen(filename,
"r" )) == NULL)
1079 "Impossible to read the config file.");
1082 while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
1084 if (
'#' == Ligne[0]) {
continue; }
1085 sscanf(Ligne,
"%s", namoption);
1086 dim = strlen(namoption);
1089 NULL != opt_Afma6[code];
1092 if (strncmp(opt_Afma6[code], namoption, dim) == 0)
1101 sscanf(Ligne,
"%s %lf %lf %lf %lf %lf %lf",
1109 sscanf(Ligne,
"%s %lf %lf %lf %lf %lf %lf", namoption,
1116 sscanf(Ligne,
"%s %lf", namoption, &this->
_long_56);
1120 sscanf(Ligne,
"%s %lf", namoption, &this->
_coupl_56);
1127 sscanf(Ligne,
"%s %lf %lf %lf", namoption,
1133 rot_eMc[0] *= M_PI / 180.0;
1134 rot_eMc[1] *= M_PI / 180.0;
1135 rot_eMc[2] *= M_PI / 180.0;
1140 sscanf(Ligne,
"%s %lf %lf %lf", namoption,
1144 get_trans_eMc =
true;
1149 "ligne #%d.", filename, numLn);
1156 if (get_rot_eMc && get_trans_eMc) {
1157 for (
unsigned int i=0; i < 3; i ++) {
1158 _erc[i] = rot_eMc[i];
1159 _etc[i] = trans_eMc[i];
1235 const unsigned int &image_width,
1236 const unsigned int &image_height)
1238 #if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
1242 std::cout <<
"Get camera parameters for camera \""
1244 <<
"from the XML file: \""
1246 if (parser.
parse(cam,
1252 "Impossible to read the camera parameters.");
1257 std::cout <<
"Get camera parameters for camera \""
1259 <<
"from the XML file: \""
1261 if (parser.
parse(cam,
1267 "Impossible to read the camera parameters.");
1272 std::cout <<
"Get camera parameters for camera \""
1274 <<
"from the XML file: \""
1276 if (parser.
parse(cam,
1282 "Impossible to read the camera parameters.");
1287 std::cout <<
"Get camera parameters for camera \""
1289 <<
"from the XML file: \""
1291 if (parser.
parse(cam,
1297 "Impossible to read the camera parameters.");
1312 "Impossible to read the camera parameters.");
1321 if (image_width == 640 && image_height == 480) {
1322 std::cout <<
"Get default camera parameters for camera \""
1334 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1336 "Impossible to read the camera parameters.");
1342 if (image_width == 640 && image_height == 480) {
1343 std::cout <<
"Get default camera parameters for camera \""
1355 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1357 "Impossible to read the camera parameters.");
1363 if (image_width == 640 && image_height == 480) {
1364 std::cout <<
"Get default camera parameters for camera \""
1376 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1378 "Impossible to read the camera parameters.");
1384 if (image_width == 640 && image_height == 480) {
1385 std::cout <<
"Get default camera parameters for camera \""
1397 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1399 "Impossible to read the camera parameters.");
1526 <<
"Joint Max:" << std::endl
1533 <<
"\t" << std::endl
1535 <<
"Joint Min: " << std::endl
1542 <<
"\t" << std::endl
1544 <<
"Long 5-6: " << std::endl
1546 <<
"\t" << std::endl
1548 <<
"Coupling 5-6:" << std::endl
1550 <<
"\t" << std::endl
1552 <<
"eMc: "<< std::endl
1553 <<
"\tTranslation (m): "
1554 << afma6.
_eMc[0][3] <<
" "
1555 << afma6.
_eMc[1][3] <<
" "
1557 <<
"\t" << std::endl
1558 <<
"\tRotation Rxyz (rad) : "
1562 <<
"\t" << std::endl
1563 <<
"\tRotation Rxyz (deg) : "
1567 <<
"\t" << std::endl;