50 #include <visp3/core/vpCameraParameters.h> 51 #include <visp3/core/vpDebug.h> 52 #include <visp3/core/vpRotationMatrix.h> 53 #include <visp3/core/vpRxyzVector.h> 54 #include <visp3/core/vpTranslationVector.h> 55 #include <visp3/core/vpVelocityTwistMatrix.h> 56 #include <visp3/core/vpXmlParserCamera.h> 57 #include <visp3/robot/vpAfma6.h> 58 #include <visp3/robot/vpRobotException.h> 64 static const char *opt_Afma6[] = {
"JOINT_MAX",
"JOINT_MIN",
"LONG_56",
"COUPL_56",
65 "CAMERA",
"eMc_ROT_XYZ",
"eMc_TRANS_XYZ", NULL};
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(), tool_current(
vpAfma6::defaultTool),
171 void vpAfma6::init(
const std::string &camera_extrinsic_parameters,
const std::string &camera_intrinsic_parameters)
271 #ifdef VISP_HAVE_AFMA6_DATA 273 std::string filename_eMc;
335 #else // VISP_HAVE_AFMA6_DATA 421 #endif // VISP_HAVE_AFMA6_DATA 533 const bool &verbose)
const 536 double q_[2][6], d[2], t;
560 if (fMe[2][2] >= .99999f) {
562 q_[0][4] = q_[1][4] = M_PI / 2.f;
563 t = atan2(fMe[0][0], fMe[0][1]);
564 q_[1][3] = q_[0][3] = q[3];
565 q_[1][5] = q_[0][5] = t - q_[0][3];
577 }
else if (fMe[2][2] <= -.99999) {
579 q_[0][4] = q_[1][4] = -M_PI / 2;
580 t = atan2(fMe[1][1], fMe[1][0]);
581 q_[1][3] = q_[0][3] = q[3];
582 q_[1][5] = q_[0][5] = q_[0][3] - t;
594 q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
596 q_[1][3] = q_[0][3] - M_PI;
598 q_[1][3] = q_[0][3] + M_PI;
600 q_[0][4] = asin(fMe[2][2]);
602 q_[1][4] = M_PI - q_[0][4];
604 q_[1][4] = -M_PI - q_[0][4];
606 q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
608 q_[1][5] = q_[0][5] - M_PI;
610 q_[1][5] = q_[0][5] + M_PI;
612 q_[0][0] = fMe[0][3] - this->
_long_56 * cos(q_[0][3]);
613 q_[1][0] = fMe[0][3] - this->
_long_56 * cos(q_[1][3]);
614 q_[0][1] = fMe[1][3] - this->
_long_56 * sin(q_[0][3]);
615 q_[1][1] = fMe[1][3] - this->
_long_56 * sin(q_[1][3]);
616 q_[0][2] = q_[1][2] = fMe[2][3];
622 for (
int j = 0; j < 2; j++) {
625 for (
unsigned int i = 0; i < 6; i++) {
629 std::cout <<
"Joint " << i <<
" not in limits: " << this->
_joint_min[i] <<
" < " << q_[j][i] <<
" < " 641 std::cout <<
"No solution..." << std::endl;
644 }
else if (ok[1] == 1) {
645 for (
unsigned int i = 0; i < 6; i++)
651 for (
unsigned int i = 0; i < 6; i++)
657 for (
int j = 0; j < 2; j++) {
659 for (
unsigned int i = 3; i < 6; i++)
660 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
662 if (nearest ==
true) {
664 for (
unsigned int i = 0; i < 6; i++)
667 for (
unsigned int i = 0; i < 6; i++)
671 for (
unsigned int i = 0; i < 6; i++)
674 for (
unsigned int i = 0; i < 6; i++)
679 for (
unsigned int i = 0; i < 6; i++)
743 fMc = fMe * this->
_eMc;
774 double q5 = q[5] - this->
_coupl_56 * q[4];
776 double c1 = cos(q[3]);
777 double s1 = sin(q[3]);
778 double c2 = cos(q[4]);
779 double s2 = sin(q[4]);
785 fMe[0][0] = s1 * s2 * c3 + c1 * s3;
786 fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
787 fMe[0][2] = -s1 * c2;
788 fMe[0][3] = q0 + this->
_long_56 * c1;
790 fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
791 fMe[1][1] = c1 * s2 * s3 + s1 * c3;
793 fMe[1][3] = q1 + this->
_long_56 * s1;
796 fMe[2][1] = -c2 * s3;
869 double s4, c4, s5, c5, s6, c6;
879 eJe[0][0] = s4 * s5 * c6 + c4 * s6;
880 eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
882 eJe[0][3] = -this->
_long_56 * s5 * c6;
884 eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
885 eJe[1][1] = c4 * s5 * s6 + s4 * c6;
886 eJe[1][2] = -c5 * s6;
887 eJe[1][3] = this->
_long_56 * s5 * s6;
889 eJe[2][0] = -s4 * c5;
897 eJe[4][3] = -c5 * s6;
940 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
942 double s4 = sin(q[3]);
943 double c4 = cos(q[3]);
949 double s5 = sin(q[4]);
950 double c5 = cos(q[4]);
953 fJe[3][5] = -s4 * c5;
978 for (
unsigned int i = 0; i < 6; i++)
994 for (
unsigned int i = 0; i < 6; i++)
1030 std::ifstream fdconfig(filename.c_str(), std::ios::in);
1032 if (!fdconfig.is_open()) {
1039 bool get_erc =
false;
1040 bool get_etc =
false;
1043 while (std::getline(fdconfig, line)) {
1045 if ((line.compare(0, 1,
"#") == 0) || line.empty()) {
1048 std::istringstream ss(line);
1052 for (code = 0; NULL != opt_Afma6[code]; ++code) {
1053 if (key.compare(opt_Afma6[code]) == 0) {
1081 ss >> erc[0] >> erc[1] >> erc[2];
1084 erc = erc * M_PI / 180.0;
1089 ss >> etc[0] >> etc[1] >> etc[2];
1095 filename.c_str(), lineNum));
1102 if (get_etc && get_erc) {
1191 const unsigned int &image_height)
const 1193 #if defined(VISP_HAVE_XML2) && defined(VISP_HAVE_AFMA6_DATA) 1250 if (image_width == 640 && image_height == 480) {
1262 vpTRACE(
"Cannot get default intrinsic camera parameters for this image " 1270 if (image_width == 640 && image_height == 480) {
1282 vpTRACE(
"Cannot get default intrinsic camera parameters for this image " 1290 if (image_width == 640 && image_height == 480) {
1302 vpTRACE(
"Cannot get default intrinsic camera parameters for this image " 1310 if (image_width == 640 && image_height == 480) {
1322 vpTRACE(
"Cannot get default intrinsic camera parameters for this image " 1446 os <<
"Joint Max:" << std::endl
1450 <<
"Joint Min: " << std::endl
1454 <<
"Long 5-6: " << std::endl
1455 <<
"\t" << afma6.
_long_56 <<
"\t" << std::endl
1457 <<
"Coupling 5-6:" << std::endl
1458 <<
"\t" << afma6.
_coupl_56 <<
"\t" << std::endl
1460 <<
"eMc: " << std::endl
1461 <<
"\tTranslation (m): " << afma6.
_eMc[0][3] <<
" " << afma6.
_eMc[1][3] <<
" " << afma6.
_eMc[2][3] <<
"\t" 1463 <<
"\tRotation Rxyz (rad) : " << rxyz[0] <<
" " << rxyz[1] <<
" " << rxyz[2] <<
"\t" << std::endl
1465 <<
"\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.
unsigned int getWidth() const
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
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)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
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
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.
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
unsigned int getRows() const
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
Class that consider the case of a translation vector.
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