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_eMc_Intel_D435_without_distortion_Afma6.cnf");
99 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf");
102 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_camera_Afma6.xml");
122 : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(
vpAfma6::defaultTool),
178 void vpAfma6::init(
const std::string &camera_extrinsic_parameters,
const std::string &camera_intrinsic_parameters)
278 #ifdef VISP_HAVE_AFMA6_DATA
280 std::string filename_eMc;
292 "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
307 "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
322 "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
337 "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
352 "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
397 "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
422 "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
447 "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
472 "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
492 "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
611 const bool &verbose)
const
614 double q_[2][6], d[2], t;
638 if (fMe[2][2] >= .99999f) {
640 q_[0][4] = q_[1][4] = M_PI / 2.f;
641 t = atan2(fMe[0][0], fMe[0][1]);
642 q_[1][3] = q_[0][3] = q[3];
643 q_[1][5] = q_[0][5] = t - q_[0][3];
655 }
else if (fMe[2][2] <= -.99999) {
657 q_[0][4] = q_[1][4] = -M_PI / 2;
658 t = atan2(fMe[1][1], fMe[1][0]);
659 q_[1][3] = q_[0][3] = q[3];
660 q_[1][5] = q_[0][5] = q_[0][3] - t;
672 q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
674 q_[1][3] = q_[0][3] - M_PI;
676 q_[1][3] = q_[0][3] + M_PI;
678 q_[0][4] = asin(fMe[2][2]);
680 q_[1][4] = M_PI - q_[0][4];
682 q_[1][4] = -M_PI - q_[0][4];
684 q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
686 q_[1][5] = q_[0][5] - M_PI;
688 q_[1][5] = q_[0][5] + M_PI;
690 q_[0][0] = fMe[0][3] - this->
_long_56 * cos(q_[0][3]);
691 q_[1][0] = fMe[0][3] - this->
_long_56 * cos(q_[1][3]);
692 q_[0][1] = fMe[1][3] - this->
_long_56 * sin(q_[0][3]);
693 q_[1][1] = fMe[1][3] - this->
_long_56 * sin(q_[1][3]);
694 q_[0][2] = q_[1][2] = fMe[2][3];
700 for (
int j = 0; j < 2; j++) {
703 for (
unsigned int i = 0; i < 6; i++) {
707 std::cout <<
"Joint " << i <<
" not in limits: " << this->
_joint_min[i] <<
" < " << q_[j][i] <<
" < "
719 std::cout <<
"No solution..." << std::endl;
722 }
else if (ok[1] == 1) {
723 for (
unsigned int i = 0; i < 6; i++)
729 for (
unsigned int i = 0; i < 6; i++)
735 for (
int j = 0; j < 2; j++) {
737 for (
unsigned int i = 3; i < 6; i++)
738 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
740 if (nearest ==
true) {
742 for (
unsigned int i = 0; i < 6; i++)
745 for (
unsigned int i = 0; i < 6; i++)
749 for (
unsigned int i = 0; i < 6; i++)
752 for (
unsigned int i = 0; i < 6; i++)
757 for (
unsigned int i = 0; i < 6; i++)
821 fMc = fMe * this->
_eMc;
852 double q5 = q[5] - this->
_coupl_56 * q[4];
854 double c1 = cos(q[3]);
855 double s1 = sin(q[3]);
856 double c2 = cos(q[4]);
857 double s2 = sin(q[4]);
863 fMe[0][0] = s1 * s2 * c3 + c1 * s3;
864 fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
865 fMe[0][2] = -s1 * c2;
866 fMe[0][3] = q0 + this->
_long_56 * c1;
868 fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
869 fMe[1][1] = c1 * s2 * s3 + s1 * c3;
871 fMe[1][3] = q1 + this->
_long_56 * s1;
874 fMe[2][1] = -c2 * s3;
947 double s4, c4, s5, c5, s6, c6;
957 eJe[0][0] = s4 * s5 * c6 + c4 * s6;
958 eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
960 eJe[0][3] = -this->
_long_56 * s5 * c6;
962 eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
963 eJe[1][1] = c4 * s5 * s6 + s4 * c6;
964 eJe[1][2] = -c5 * s6;
965 eJe[1][3] = this->
_long_56 * s5 * s6;
967 eJe[2][0] = -s4 * c5;
975 eJe[4][3] = -c5 * s6;
1018 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1020 double s4 = sin(q[3]);
1021 double c4 = cos(q[3]);
1027 double s5 = sin(q[4]);
1028 double c5 = cos(q[4]);
1031 fJe[3][5] = -s4 * c5;
1033 fJe[4][5] = c4 * c5;
1039 fJe[4][4] += -this->
_coupl_56 * c4 * c5;
1056 for (
unsigned int i = 0; i < 6; i++)
1072 for (
unsigned int i = 0; i < 6; i++)
1108 std::ifstream fdconfig(filename.c_str(), std::ios::in);
1110 if (!fdconfig.is_open()) {
1117 bool get_erc =
false;
1118 bool get_etc =
false;
1121 while (std::getline(fdconfig, line)) {
1123 if ((line.compare(0, 1,
"#") == 0) || line.empty()) {
1126 std::istringstream ss(line);
1130 for (code = 0; NULL != opt_Afma6[code]; ++code) {
1131 if (key.compare(opt_Afma6[code]) == 0) {
1159 ss >> erc[0] >> erc[1] >> erc[2];
1162 erc = erc * M_PI / 180.0;
1167 ss >> etc[0] >> etc[1] >> etc[2];
1173 filename.c_str(), lineNum));
1180 if (get_etc && get_erc) {
1267 const unsigned int &image_height)
const
1269 #if defined(VISP_HAVE_AFMA6_DATA)
1335 if (image_width == 640 && image_height == 480) {
1347 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1351 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1359 if (image_width == 640 && image_height == 480) {
1371 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1375 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1383 if (image_width == 640 && image_height == 480) {
1395 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1399 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1407 if (image_width == 640 && image_height == 480) {
1419 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1423 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1431 if (image_width == 640 && image_height == 480) {
1443 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1447 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1570 os <<
"Joint Max:" << std::endl
1574 <<
"Joint Min: " << std::endl
1578 <<
"Long 5-6: " << std::endl
1579 <<
"\t" << afma6.
_long_56 <<
"\t" << std::endl
1581 <<
"Coupling 5-6:" << std::endl
1582 <<
"\t" << afma6.
_coupl_56 <<
"\t" << std::endl
1584 <<
"eMc: " << std::endl
1585 <<
"\tTranslation (m): " << afma6.
_eMc[0][3] <<
" " << afma6.
_eMc[1][3] <<
" " << afma6.
_eMc[2][3] <<
"\t"
1587 <<
"\tRotation Rxyz (rad) : " << rxyz[0] <<
" " << rxyz[1] <<
" " << rxyz[2] <<
"\t" << std::endl
1589 <<
"\t" << std::endl;
Modelisation of Irisa's gantry robot named Afma6.
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
static const char *const CONST_CCMOP_CAMERA_NAME
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
void get_cVe(vpVelocityTwistMatrix &cVe) const
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
vpColVector getJointMax() const
static const std::string CONST_CAMERA_AFMA6_FILENAME
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
static const unsigned int njoint
Number of joint.
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
static const char *const CONST_VACUUM_CAMERA_NAME
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
void get_cMe(vpHomogeneousMatrix &cMe) const
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
vpAfma6ToolType getToolType() const
Get the current tool type.
static const std::string CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME
vpColVector getJointMin() const
static const char *const CONST_INTEL_D435_CAMERA_NAME
double getCoupl56() const
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
static const char *const CONST_GRIPPER_CAMERA_NAME
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
void parseConfigFile(const std::string &filename)
vpHomogeneousMatrix get_eMc() const
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
static const char *const CONST_GENERIC_CAMERA_NAME
vpCameraParameters::vpCameraParametersProjType projModel
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
static const std::string CONST_AFMA6_FILENAME
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
unsigned int getRows() const
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
vpCameraParametersProjType
@ perspectiveProjWithDistortion
@ ProjWithKannalaBrandtDistortion
@ perspectiveProjWithoutDistortion
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emited by ViSP classes.
@ notImplementedError
Not implemented.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
unsigned int getWidth() const
unsigned int getHeight() const
static double rad(double deg)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
Error that can be emited by the vpRobot class and its derivates.
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0)