47 #include <visp3/core/vpCameraParameters.h>
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpRotationMatrix.h>
50 #include <visp3/core/vpRxyzVector.h>
51 #include <visp3/core/vpTranslationVector.h>
52 #include <visp3/core/vpVelocityTwistMatrix.h>
53 #include <visp3/core/vpXmlParserCamera.h>
54 #include <visp3/robot/vpAfma6.h>
55 #include <visp3/robot/vpRobotException.h>
61 static const char *opt_Afma6[] = {
"JOINT_MAX",
"JOINT_MIN",
"LONG_56",
"COUPL_56",
62 "CAMERA",
"eMc_ROT_XYZ",
"eMc_TRANS_XYZ",
nullptr };
64 #ifdef VISP_HAVE_AFMA6_DATA
66 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_Afma6.cnf");
69 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_ccmop_without_distortion_Afma6.cnf");
72 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_ccmop_with_distortion_Afma6.cnf");
75 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_gripper_without_distortion_Afma6.cnf");
78 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_gripper_with_distortion_Afma6.cnf");
81 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_vacuum_without_distortion_Afma6.cnf");
84 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_vacuum_with_distortion_Afma6.cnf");
87 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_generic_without_distortion_Afma6.cnf");
90 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_generic_with_distortion_Afma6.cnf");
93 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf");
96 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf");
99 std::string(VISP_AFMA6_DATA_PATH) + std::string(
"/include/const_camera_Afma6.xml");
119 : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(
vpAfma6::defaultTool),
175 void vpAfma6::init(
const std::string &camera_extrinsic_parameters,
const std::string &camera_intrinsic_parameters)
275 #ifdef VISP_HAVE_AFMA6_DATA
277 std::string filename_eMc;
289 "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
304 "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
319 "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
334 "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
349 "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
386 "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
411 "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
436 "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
461 "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
481 "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
600 const bool &verbose)
const
603 double q_[2][6], d[2], t;
627 if (fMe[2][2] >= .99999f) {
629 q_[0][4] = q_[1][4] = M_PI / 2.f;
630 t = atan2(fMe[0][0], fMe[0][1]);
631 q_[1][3] = q_[0][3] = q[3];
632 q_[1][5] = q_[0][5] = t - q_[0][3];
645 else if (fMe[2][2] <= -.99999) {
647 q_[0][4] = q_[1][4] = -M_PI / 2;
648 t = atan2(fMe[1][1], fMe[1][0]);
649 q_[1][3] = q_[0][3] = q[3];
650 q_[1][5] = q_[0][5] = q_[0][3] - t;
663 q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
665 q_[1][3] = q_[0][3] - M_PI;
667 q_[1][3] = q_[0][3] + M_PI;
669 q_[0][4] = asin(fMe[2][2]);
671 q_[1][4] = M_PI - q_[0][4];
673 q_[1][4] = -M_PI - q_[0][4];
675 q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
677 q_[1][5] = q_[0][5] - M_PI;
679 q_[1][5] = q_[0][5] + M_PI;
681 q_[0][0] = fMe[0][3] - this->
_long_56 * cos(q_[0][3]);
682 q_[1][0] = fMe[0][3] - this->
_long_56 * cos(q_[1][3]);
683 q_[0][1] = fMe[1][3] - this->
_long_56 * sin(q_[0][3]);
684 q_[1][1] = fMe[1][3] - this->
_long_56 * sin(q_[1][3]);
685 q_[0][2] = q_[1][2] = fMe[2][3];
691 for (
int j = 0; j < 2; j++) {
694 for (
unsigned int i = 0; i < 6; i++) {
698 std::cout <<
"Joint " << i <<
" not in limits: " << this->
_joint_min[i] <<
" < " << q_[j][i] <<
" < "
710 std::cout <<
"No solution..." << std::endl;
714 else if (ok[1] == 1) {
715 for (
unsigned int i = 0; i < 6; i++)
722 for (
unsigned int i = 0; i < 6; i++)
729 for (
int j = 0; j < 2; j++) {
731 for (
unsigned int i = 3; i < 6; i++)
732 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
734 if (nearest ==
true) {
736 for (
unsigned int i = 0; i < 6; i++)
739 for (
unsigned int i = 0; i < 6; i++)
744 for (
unsigned int i = 0; i < 6; i++)
747 for (
unsigned int i = 0; i < 6; i++)
752 for (
unsigned int i = 0; i < 6; i++)
816 fMc = fMe * this->
_eMc;
847 double q5 = q[5] - this->
_coupl_56 * q[4];
849 double c1 = cos(q[3]);
850 double s1 = sin(q[3]);
851 double c2 = cos(q[4]);
852 double s2 = sin(q[4]);
858 fMe[0][0] = s1 * s2 * c3 + c1 * s3;
859 fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
860 fMe[0][2] = -s1 * c2;
861 fMe[0][3] = q0 + this->
_long_56 * c1;
863 fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
864 fMe[1][1] = c1 * s2 * s3 + s1 * c3;
866 fMe[1][3] = q1 + this->
_long_56 * s1;
869 fMe[2][1] = -c2 * s3;
942 double s4, c4, s5, c5, s6, c6;
952 eJe[0][0] = s4 * s5 * c6 + c4 * s6;
953 eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
955 eJe[0][3] = -this->
_long_56 * s5 * c6;
957 eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
958 eJe[1][1] = c4 * s5 * s6 + s4 * c6;
959 eJe[1][2] = -c5 * s6;
960 eJe[1][3] = this->
_long_56 * s5 * s6;
962 eJe[2][0] = -s4 * c5;
970 eJe[4][3] = -c5 * s6;
1013 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1015 double s4 = sin(q[3]);
1016 double c4 = cos(q[3]);
1022 double s5 = sin(q[4]);
1023 double c5 = cos(q[4]);
1026 fJe[3][5] = -s4 * c5;
1028 fJe[4][5] = c4 * c5;
1034 fJe[4][4] += -this->
_coupl_56 * c4 * c5;
1051 for (
unsigned int i = 0; i < 6; i++)
1067 for (
unsigned int i = 0; i < 6; i++)
1103 std::ifstream fdconfig(filename.c_str(), std::ios::in);
1105 if (!fdconfig.is_open()) {
1112 bool get_erc =
false;
1113 bool get_etc =
false;
1116 while (std::getline(fdconfig, line)) {
1118 if ((line.compare(0, 1,
"#") == 0) || line.empty()) {
1121 std::istringstream ss(line);
1125 for (code = 0;
nullptr != opt_Afma6[code]; ++code) {
1126 if (key.compare(opt_Afma6[code]) == 0) {
1154 ss >> erc[0] >> erc[1] >> erc[2];
1157 erc = erc * M_PI / 180.0;
1162 ss >> etc[0] >> etc[1] >> etc[2];
1168 filename.c_str(), lineNum));
1175 if (get_etc && get_erc) {
1262 const unsigned int &image_height)
const
1264 #if defined(VISP_HAVE_AFMA6_DATA) && defined(VISP_HAVE_PUGIXML)
1330 if (image_width == 640 && image_height == 480) {
1342 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1347 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1355 if (image_width == 640 && image_height == 480) {
1367 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1372 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1380 if (image_width == 640 && image_height == 480) {
1392 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1397 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1405 if (image_width == 640 && image_height == 480) {
1417 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1422 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1430 if (image_width == 640 && image_height == 480) {
1442 "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;
Modelization 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
Perspective projection with distortion model.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
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 emitted 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 emitted by the vpRobot class and its derivatives.
@ readingParametersError
Cannot parse parameters.
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, bool verbose=true)