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];
644 }
else if (fMe[2][2] <= -.99999) {
646 q_[0][4] = q_[1][4] = -M_PI / 2;
647 t = atan2(fMe[1][1], fMe[1][0]);
648 q_[1][3] = q_[0][3] = q[3];
649 q_[1][5] = q_[0][5] = q_[0][3] - t;
661 q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
663 q_[1][3] = q_[0][3] - M_PI;
665 q_[1][3] = q_[0][3] + M_PI;
667 q_[0][4] = asin(fMe[2][2]);
669 q_[1][4] = M_PI - q_[0][4];
671 q_[1][4] = -M_PI - q_[0][4];
673 q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
675 q_[1][5] = q_[0][5] - M_PI;
677 q_[1][5] = q_[0][5] + M_PI;
679 q_[0][0] = fMe[0][3] - this->
_long_56 * cos(q_[0][3]);
680 q_[1][0] = fMe[0][3] - this->
_long_56 * cos(q_[1][3]);
681 q_[0][1] = fMe[1][3] - this->
_long_56 * sin(q_[0][3]);
682 q_[1][1] = fMe[1][3] - this->
_long_56 * sin(q_[1][3]);
683 q_[0][2] = q_[1][2] = fMe[2][3];
689 for (
int j = 0; j < 2; j++) {
692 for (
unsigned int i = 0; i < 6; i++) {
696 std::cout <<
"Joint " << i <<
" not in limits: " << this->
_joint_min[i] <<
" < " << q_[j][i] <<
" < "
708 std::cout <<
"No solution..." << std::endl;
711 }
else if (ok[1] == 1) {
712 for (
unsigned int i = 0; i < 6; i++)
718 for (
unsigned int i = 0; i < 6; i++)
724 for (
int j = 0; j < 2; j++) {
726 for (
unsigned int i = 3; i < 6; i++)
727 d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
729 if (nearest ==
true) {
731 for (
unsigned int i = 0; i < 6; i++)
734 for (
unsigned int i = 0; i < 6; i++)
738 for (
unsigned int i = 0; i < 6; i++)
741 for (
unsigned int i = 0; i < 6; i++)
746 for (
unsigned int i = 0; i < 6; i++)
810 fMc = fMe * this->
_eMc;
841 double q5 = q[5] - this->
_coupl_56 * q[4];
843 double c1 = cos(q[3]);
844 double s1 = sin(q[3]);
845 double c2 = cos(q[4]);
846 double s2 = sin(q[4]);
852 fMe[0][0] = s1 * s2 * c3 + c1 * s3;
853 fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
854 fMe[0][2] = -s1 * c2;
855 fMe[0][3] = q0 + this->
_long_56 * c1;
857 fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
858 fMe[1][1] = c1 * s2 * s3 + s1 * c3;
860 fMe[1][3] = q1 + this->
_long_56 * s1;
863 fMe[2][1] = -c2 * s3;
936 double s4, c4, s5, c5, s6, c6;
946 eJe[0][0] = s4 * s5 * c6 + c4 * s6;
947 eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
949 eJe[0][3] = -this->
_long_56 * s5 * c6;
951 eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
952 eJe[1][1] = c4 * s5 * s6 + s4 * c6;
953 eJe[1][2] = -c5 * s6;
954 eJe[1][3] = this->
_long_56 * s5 * s6;
956 eJe[2][0] = -s4 * c5;
964 eJe[4][3] = -c5 * s6;
1007 fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1009 double s4 = sin(q[3]);
1010 double c4 = cos(q[3]);
1016 double s5 = sin(q[4]);
1017 double c5 = cos(q[4]);
1020 fJe[3][5] = -s4 * c5;
1022 fJe[4][5] = c4 * c5;
1028 fJe[4][4] += -this->
_coupl_56 * c4 * c5;
1045 for (
unsigned int i = 0; i < 6; i++)
1061 for (
unsigned int i = 0; i < 6; i++)
1097 std::ifstream fdconfig(filename.c_str(), std::ios::in);
1099 if (!fdconfig.is_open()) {
1106 bool get_erc =
false;
1107 bool get_etc =
false;
1110 while (std::getline(fdconfig, line)) {
1112 if ((line.compare(0, 1,
"#") == 0) || line.empty()) {
1115 std::istringstream ss(line);
1119 for (code = 0;
nullptr != opt_Afma6[code]; ++code) {
1120 if (key.compare(opt_Afma6[code]) == 0) {
1148 ss >> erc[0] >> erc[1] >> erc[2];
1151 erc = erc * M_PI / 180.0;
1156 ss >> etc[0] >> etc[1] >> etc[2];
1162 filename.c_str(), lineNum));
1169 if (get_etc && get_erc) {
1256 const unsigned int &image_height)
const
1258 #if defined(VISP_HAVE_AFMA6_DATA)
1324 if (image_width == 640 && image_height == 480) {
1336 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1340 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1348 if (image_width == 640 && image_height == 480) {
1360 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1364 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1372 if (image_width == 640 && image_height == 480) {
1384 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1388 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1396 if (image_width == 640 && image_height == 480) {
1408 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1412 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1420 if (image_width == 640 && image_height == 480) {
1432 "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1436 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
1559 os <<
"Joint Max:" << std::endl
1563 <<
"Joint Min: " << std::endl
1567 <<
"Long 5-6: " << std::endl
1568 <<
"\t" << afma6.
_long_56 <<
"\t" << std::endl
1570 <<
"Coupling 5-6:" << std::endl
1571 <<
"\t" << afma6.
_coupl_56 <<
"\t" << std::endl
1573 <<
"eMc: " << std::endl
1574 <<
"\tTranslation (m): " << afma6.
_eMc[0][3] <<
" " << afma6.
_eMc[1][3] <<
" " << afma6.
_eMc[2][3] <<
"\t"
1576 <<
"\tRotation Rxyz (rad) : " << rxyz[0] <<
" " << rxyz[1] <<
" " << rxyz[2] <<
"\t" << std::endl
1578 <<
"\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)