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)
1071 "Impossible to read the config file.");
1074 while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
1076 if (
'#' == Ligne[0]) {
continue; }
1077 sscanf(Ligne,
"%s", namoption);
1078 dim = strlen(namoption);
1081 NULL != opt_Afma6[code];
1084 if (strncmp(opt_Afma6[code], namoption, dim) == 0)
1093 sscanf(Ligne,
"%s %lf %lf %lf %lf %lf %lf",
1101 sscanf(Ligne,
"%s %lf %lf %lf %lf %lf %lf", namoption,
1108 sscanf(Ligne,
"%s %lf", namoption, &this->
_long_56);
1112 sscanf(Ligne,
"%s %lf", namoption, &this->
_coupl_56);
1119 sscanf(Ligne,
"%s %lf %lf %lf", namoption,
1125 rot_eMc[0] *= M_PI / 180.0;
1126 rot_eMc[1] *= M_PI / 180.0;
1127 rot_eMc[2] *= M_PI / 180.0;
1132 sscanf(Ligne,
"%s %lf %lf %lf", namoption,
1136 get_trans_eMc =
true;
1141 "ligne #%d.", filename, numLn);
1148 if (get_rot_eMc && get_trans_eMc) {
1149 for (
unsigned int i=0; i < 3; i ++) {
1150 _erc[i] = rot_eMc[i];
1151 _etc[i] = trans_eMc[i];
1227 const unsigned int &image_width,
1228 const unsigned int &image_height)
1230 #if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
1234 std::cout <<
"Get camera parameters for camera \""
1236 <<
"from the XML file: \""
1238 if (parser.
parse(cam,
1244 "Impossible to read the camera parameters.");
1249 std::cout <<
"Get camera parameters for camera \""
1251 <<
"from the XML file: \""
1253 if (parser.
parse(cam,
1259 "Impossible to read the camera parameters.");
1264 std::cout <<
"Get camera parameters for camera \""
1266 <<
"from the XML file: \""
1268 if (parser.
parse(cam,
1274 "Impossible to read the camera parameters.");
1279 std::cout <<
"Get camera parameters for camera \""
1281 <<
"from the XML file: \""
1283 if (parser.
parse(cam,
1289 "Impossible to read the camera parameters.");
1304 "Impossible to read the camera parameters.");
1313 if (image_width == 640 && image_height == 480) {
1314 std::cout <<
"Get default camera parameters for camera \""
1326 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1328 "Impossible to read the camera parameters.");
1334 if (image_width == 640 && image_height == 480) {
1335 std::cout <<
"Get default camera parameters for camera \""
1347 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1349 "Impossible to read the camera parameters.");
1355 if (image_width == 640 && image_height == 480) {
1356 std::cout <<
"Get default camera parameters for camera \""
1368 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1370 "Impossible to read the camera parameters.");
1376 if (image_width == 640 && image_height == 480) {
1377 std::cout <<
"Get default camera parameters for camera \""
1389 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
1391 "Impossible to read the camera parameters.");
1518 <<
"Joint Max:" << std::endl
1525 <<
"\t" << std::endl
1527 <<
"Joint Min: " << std::endl
1534 <<
"\t" << std::endl
1536 <<
"Long 5-6: " << std::endl
1538 <<
"\t" << std::endl
1540 <<
"Coupling 5-6:" << std::endl
1542 <<
"\t" << std::endl
1544 <<
"eMc: "<< std::endl
1545 <<
"\tTranslation (m): "
1546 << afma6.
_eMc[0][3] <<
" "
1547 << afma6.
_eMc[1][3] <<
" "
1549 <<
"\t" << std::endl
1550 <<
"\tRotation Rxyz (rad) : "
1554 <<
"\t" << std::endl
1555 <<
"\tRotation Rxyz (deg) : "
1559 <<
"\t" << std::endl;
Modelisation of Irisa's gantry robot named Afma6.
Definition of the vpMatrix class.
static const unsigned int njoint
Number of joint.
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Error that can be emited by the vpRobot class and its derivates.
static const char *const CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Perspective projection without distortion model.
static const char *const CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
unsigned int getWidth() const
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void get_cVe(vpVelocityTwistMatrix &cVe)
void setIdentity()
Basic initialisation (identity)
vpHomogeneousMatrix get_fMc(const vpColVector &q)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
vpCameraParameters::vpCameraParametersProjType projModel
static const char *const CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
XML parser to load and save intrinsic camera parameters.
vpAfma6ToolType getToolType()
Get the current tool type.
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
static const char *const CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
static const char *const CONST_GRIPPER_CAMERA_NAME
The vpRotationMatrix considers the particular case of a rotation matrix.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void get_cMe(vpHomogeneousMatrix &cMe)
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
static const char *const CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
static const char *const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
void get_eJe(const vpColVector &q, vpMatrix &eJe)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static const char *const CONST_CAMERA_AFMA6_FILENAME
vpCameraParametersProjType
static const char *const CONST_AFMA6_FILENAME
Generic class defining intrinsic camera parameters.
void parseConfigFile(const char *filename)
void extract(vpRotationMatrix &R) const
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q)
VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpImagePoint &ip)
Perspective projection with distortion model.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static double rad(double deg)
int parse(vpCameraParameters &cam, const char *filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true)
static double deg(double rad)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
static const char *const CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
vpHomogeneousMatrix inverse() const
vpColVector getJointMin()
void get_fJe(const vpColVector &q, vpMatrix &fJe)
unsigned int getHeight() const
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
vpColVector getJointMax()
unsigned int getRows() const
Return the number of rows of the matrix.
static const char *const CONST_CCMOP_CAMERA_NAME
static const char *const CONST_VACUUM_CAMERA_NAME
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
static const char *const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
void resize(const unsigned int i, const bool flagNullify=true)
static const char *const CONST_GENERIC_CAMERA_NAME