46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpVelocityTwistMatrix.h>
48 #include <visp3/robot/vpRobotException.h>
49 #include <visp3/core/vpCameraParameters.h>
50 #include <visp3/core/vpRxyzVector.h>
51 #include <visp3/core/vpTranslationVector.h>
52 #include <visp3/core/vpRotationMatrix.h>
53 #include <visp3/robot/vpAfma4.h>
68 : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
96 this->
_erc[1] = -M_PI/2.;
230 fMc = fMe * this->
_eMc;
284 fMe[0][0] = c1*s4*c5 + s1*c4*c5;
285 fMe[0][1] = -c1*s4*s5 - s1*c4*s5;
286 fMe[0][2] = c1*c4 - s1*s4;
287 fMe[0][3] = c1*this->
_a1 - s1*(this->
_d3);
289 fMe[1][0] = s1*s4*c5 - c1*c4*c5;
290 fMe[1][1] = -s1*s4*s5 + c1*c4*s5;
291 fMe[1][2] = s1*c4+c1*s4;
292 fMe[1][3] = s1*this->
_a1 + c1*(this->
_d3);
297 fMe[2][3] = this->
_d4 + q2;
426 eJe[0][0] = -(this->
_a1*c4 + this->
_d3*s4)*c5; eJe[0][1] = -s5;
427 eJe[1][0] = (this->
_a1*c4 + this->
_d3*s4)*s5; eJe[1][1] = -c5;
428 eJe[2][0] = (this->
_a1*s4 - this->
_d3*c4);
429 eJe[3][0] = eJe[3][2] = -s5;
430 eJe[4][0] = eJe[4][2] = -c5;
475 double c14 = cos(q1 + q4);
476 double s14 = sin(q1 + q4);
480 fJe[0][0] = -s1*this->
_a1 - c1*this->
_d3;
482 fJe[1][0] = c1*this->
_a1 - s1*this->
_d3;
490 fJe[5][0] = fJe[5][2] = 1.0;
524 fJe_inverse.
resize(4, 6) ;
532 double c14 = cos(q1 + q4);
533 double s14 = sin(q1 + q4);
535 double det = this->
_a1 * this->
_a1 + this->
_d3 * this->
_d3;
537 fJe_inverse[0][0] = (-s1*this->
_a1 - c1*this->
_d3)/det;
538 fJe_inverse[0][1] = (c1*this->
_a1 - s1*this->
_d3)/det;
540 fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;
542 fJe_inverse[2][0] = - fJe_inverse[0][0];
543 fJe_inverse[2][1] = - fJe_inverse[0][1];
545 fJe_inverse[3][3] = c14;
546 fJe_inverse[3][4] = s14;
561 for (
unsigned int i=0; i < 4; i ++)
578 for (
unsigned int i=0; i < 4; i ++)
602 <<
"Joint Max:" << std::endl
609 <<
"Joint Min: " << std::endl
616 <<
"a1: " << std::endl
620 <<
"d3: " << std::endl
624 <<
"d4: " << std::endl
628 <<
"eMc: "<< std::endl
629 <<
"\tTranslation (m): "
630 << afma4.
_eMc[0][3] <<
" "
631 << afma4.
_eMc[1][3] <<
" "
634 <<
"\tRotation Rxyz (rad) : "
639 <<
"\tRotation Rxyz (deg) : "
643 <<
"\t" << std::endl;
Implementation of a matrix and operations on matrices.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
void get_cMe(vpHomogeneousMatrix &cMe) const
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 get_cVe(vpVelocityTwistMatrix &cVe) const
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Implementation of a rotation matrix and operations on such kind of matrices.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static const unsigned int njoint
Number of joint.
Modelisation of Irisa's cylindrical robot named Afma4.
void extract(vpRotationMatrix &R) const
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Implementation of a velocity twist matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
static double deg(double rad)
Implementation of column vector and the associated operations.
vpColVector getJointMin() const
vpHomogeneousMatrix inverse() const
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Implementation of a rotation vector as Euler angle minimal representation.
void get_fJe(const vpColVector &q, vpMatrix &fJe) const