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.
static const std::string CONST_AFMA6_FILENAME
Error that can be emited by the vpRobot class and its derivates.
Perspective projection without distortion model.
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpColVector getJointMax() const
void parseConfigFile(const std::string &filename)
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
static const std::string CONST_CAMERA_AFMA6_FILENAME
void get_cVe(vpVelocityTwistMatrix &cVe) const
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
unsigned int getRows() const
Return the number of rows of the 2D array.
vpHomogeneousMatrix inverse() const
vpCameraParameters::vpCameraParametersProjType projModel
vpAfma6ToolType getToolType() const
Get the current tool type.
void extract(vpRotationMatrix &R) const
XML parser to load and save intrinsic camera parameters.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpAfma6 &afma6)
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.
vpHomogeneousMatrix get_eMc() 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_eJe(const vpColVector &q, vpMatrix &eJe) const
Generic class defining intrinsic camera parameters.
vpColVector getJointMin() const
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Implementation of a velocity twist matrix and operations on such kind of matrices.
Perspective projection with distortion model.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
static double rad(double deg)
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
vpTranslationVector getTranslationVector() const
static double deg(double rad)
unsigned int getHeight() const
Implementation of column vector and the associated operations.
void get_cMe(vpHomogeneousMatrix &cMe) 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
unsigned int getWidth() const
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
static const char *const CONST_VACUUM_CAMERA_NAME
double getCoupl56() const
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