49 #include <visp3/core/vpDebug.h>
50 #include <visp3/core/vpVelocityTwistMatrix.h>
51 #include <visp3/robot/vpRobotException.h>
52 #include <visp3/core/vpXmlParserCamera.h>
53 #include <visp3/core/vpCameraParameters.h>
54 #include <visp3/core/vpRxyzVector.h>
55 #include <visp3/core/vpTranslationVector.h>
56 #include <visp3/core/vpRotationMatrix.h>
57 #include <visp3/robot/vpAfma6.h>
63 static const char *opt_Afma6[] = {
"JOINT_MAX",
"JOINT_MIN",
"LONG_56",
"COUPL_56",
64 "CAMERA",
"eMc_ROT_XYZ",
"eMc_TRANS_XYZ",
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(),
116 tool_current(
vpAfma6::defaultTool),
175 const std::string &camera_intrinsic_parameters)
278 #ifdef VISP_HAVE_AFMA6_DATA
280 std::string filename_eMc;
342 #else // VISP_HAVE_AFMA6_DATA
428 #endif // VISP_HAVE_AFMA6_DATA
543 vpColVector & q,
const bool &nearest,
const bool &verbose)
const
546 double q_[2][6],d[2],t;
571 if (fMe[2][2] >= .99999f)
574 q_[0][4] = q_[1][4] = M_PI/2.f;
575 t = atan2(fMe[0][0],fMe[0][1]);
576 q_[1][3] = q_[0][3] = q[3];
577 q_[1][5] = q_[0][5] = t - q_[0][3];
591 else if (fMe[2][2] <= -.99999)
594 q_[0][4] = q_[1][4] = -M_PI/2;
595 t = atan2(fMe[1][1],fMe[1][0]);
596 q_[1][3] = q_[0][3] = q[3];
597 q_[1][5] = q_[0][5] = q_[0][3] - t;
612 q_[0][3] = atan2(-fMe[0][2],fMe[1][2]);
613 if (q_[0][3] >= 0.0) q_[1][3] = q_[0][3] - M_PI;
614 else q_[1][3] = q_[0][3] + M_PI;
616 q_[0][4] = asin(fMe[2][2]);
617 if (q_[0][4] >= 0.0) q_[1][4] = M_PI - q_[0][4];
618 else q_[1][4] = -M_PI - q_[0][4];
620 q_[0][5] = atan2(-fMe[2][1],fMe[2][0]);
621 if (q_[0][5] >= 0.0) q_[1][5] = q_[0][5] - M_PI;
622 else q_[1][5] = q_[0][5] + M_PI;
624 q_[0][0] = fMe[0][3] - this->
_long_56*cos(q_[0][3]);
625 q_[1][0] = fMe[0][3] - this->
_long_56*cos(q_[1][3]);
626 q_[0][1] = fMe[1][3] - this->
_long_56*sin(q_[0][3]);
627 q_[1][1] = fMe[1][3] - this->
_long_56*sin(q_[1][3]);
628 q_[0][2] = q_[1][2] = fMe[2][3];
634 for (
int j=0;j<2;j++)
638 for (
unsigned int i=0;i<6;i++) {
642 std::cout <<
"Joint " << i <<
" not in limits: " << this->
_joint_min[i] <<
" < " << q_[j][i] <<
" < " << this->
_joint_max[i] << std::endl;
653 std::cout <<
"No solution..." << std::endl;
657 else if (ok[1] == 1) {
658 for (
unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
665 for (
unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
672 for (
int j=0;j<2;j++)
675 for (
unsigned int i=3;i<6;i++)
676 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
681 for (
unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
683 for (
unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
688 for (
unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
690 for (
unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
694 for(
unsigned int i=0; i<6; i++)
760 fMc = fMe * this->
_eMc;
792 double q5 = q[5] - this->
_coupl_56 * q[4];
794 double c1 = cos(q[3]);
795 double s1 = sin(q[3]);
796 double c2 = cos(q[4]);
797 double s2 = sin(q[4]);
803 fMe[0][0] = s1*s2*c3 + c1*s3;
804 fMe[0][1] = -s1*s2*s3 + c1*c3;
808 fMe[1][0] = -c1*s2*c3 + s1*s3;
809 fMe[1][1] = c1*s2*s3 + s1*c3;
897 double s4,c4,s5,c5,s6,c6 ;
899 s4=sin(q[3]); c4=cos(q[3]);
900 s5=sin(q[4]); c5=cos(q[4]);
904 eJe[0][0] = s4*s5*c6+c4*s6;
905 eJe[0][1] = -c4*s5*c6+s4*s6;
909 eJe[1][0] = -s4*s5*s6+c4*c6;
910 eJe[1][1] = c4*s5*s6+s4*c6;
967 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1 ;
969 double s4 = sin(q[3]) ;
970 double c4 = cos(q[3]) ;
978 double s5 = sin(q[4]) ;
979 double c5 = cos(q[4]) ;
981 fJe[3][4] = c4 ; fJe[3][5] = -s4*c5 ;
982 fJe[4][4] = s4 ; fJe[4][5] = c4*c5 ;
983 fJe[5][3] = 1 ; fJe[5][5] = s5 ;
1006 for (
unsigned int i=0; i < 6; i ++)
1023 for (
unsigned int i=0; i < 6; i ++)
1066 double trans_eMc[3];
1068 std::ifstream fdconfig(filename.c_str(), std::ios::in);
1070 if(! fdconfig.is_open()) {
1072 "Impossible to read the config file: %s", filename.c_str());
1077 bool get_rot_eMc =
false;
1078 bool get_trans_eMc =
false;
1081 while(std::getline(fdconfig, line)) {
1083 if((line.compare(0, 1,
"#") == 0)) {
1086 std::istringstream ss(line);
1090 for (code = 0; NULL != opt_Afma6[code]; ++ code) {
1091 if (key.compare(opt_Afma6[code]) == 0) {
1120 ss >> rot_eMc[0] >> rot_eMc[1] >> rot_eMc[2];
1123 rot_eMc[0] *= M_PI / 180.0;
1124 rot_eMc[1] *= M_PI / 180.0;
1125 rot_eMc[2] *= M_PI / 180.0;
1130 ss >> trans_eMc[0] >> trans_eMc[1] >> trans_eMc[2];
1131 get_trans_eMc =
true;
1136 "Bad configuration file %s line #%d",
1137 filename.c_str(), lineNum));
1142 if (get_rot_eMc && get_trans_eMc) {
1143 for (
unsigned int i=0; i < 3; i ++) {
1144 _erc[i] = rot_eMc[i];
1145 _etc[i] = trans_eMc[i];
1236 const unsigned int &image_width,
1237 const unsigned int &image_height)
const
1239 #if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_AFMA6_DATA)
1243 std::cout <<
"Get camera parameters for camera \""
1245 <<
"from the XML file: \""
1247 if (parser.
parse(cam,
1253 "Impossible to read the camera parameters.");
1258 std::cout <<
"Get camera parameters for camera \""
1260 <<
"from the XML file: \""
1262 if (parser.
parse(cam,
1268 "Impossible to read the camera parameters.");
1273 std::cout <<
"Get camera parameters for camera \""
1275 <<
"from the XML file: \""
1277 if (parser.
parse(cam,
1283 "Impossible to read the camera parameters.");
1288 std::cout <<
"Get camera parameters for camera \""
1290 <<
"from the XML file: \""
1292 if (parser.
parse(cam,
1298 "Impossible to read the camera parameters.");
1313 "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;
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Modelisation of Irisa's gantry robot named Afma6.
Implementation of a matrix and operations on matrices.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
static const unsigned int njoint
Number of joint.
void get_cVe(vpVelocityTwistMatrix &cVe) const
static const std::string CONST_AFMA6_FILENAME
Error that can be emited by the vpRobot class and its derivates.
Perspective projection without distortion model.
unsigned int getWidth() const
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
vpAfma6ToolType getToolType() const
Get the current tool type.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
void parseConfigFile(const std::string &filename)
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
static const std::string CONST_CAMERA_AFMA6_FILENAME
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
vpCameraParameters::vpCameraParametersProjType projModel
XML parser to load and save intrinsic camera parameters.
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
static const char *const CONST_GRIPPER_CAMERA_NAME
Implementation of a rotation matrix and operations on such kind of matrices.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
vpColVector getJointMin() const
vpHomogeneousMatrix get_eMc() const
vpTranslationVector getTranslationVector() const
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
vpCameraParametersProjType
void get_cMe(vpHomogeneousMatrix &cMe) const
Generic class defining intrinsic camera parameters.
vpColVector getJointMax() const
void extract(vpRotationMatrix &R) const
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Implementation of a velocity twist matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
Perspective projection with distortion model.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
static double deg(double rad)
Implementation of column vector and the associated operations.
vpHomogeneousMatrix inverse() const
unsigned int getHeight() const
Implementation of a rotation vector as Euler angle minimal representation.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
static const char *const CONST_CCMOP_CAMERA_NAME
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
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)
void resize(const unsigned int i, const bool flagNullify=true)
static const char *const CONST_GENERIC_CAMERA_NAME
double getCoupl56() const