52 #include <visp/vpDebug.h>
53 #include <visp/vpVelocityTwistMatrix.h>
54 #include <visp/vpRobotException.h>
55 #include <visp/vpCameraParameters.h>
56 #include <visp/vpRxyzVector.h>
57 #include <visp/vpTranslationVector.h>
58 #include <visp/vpRotationMatrix.h>
59 #include <visp/vpViper.h>
81 c56 = -341.33 / 9102.22;
151 vpViper::convertJointPositionInLimits(
unsigned int joint,
const double &q,
double &q_mod)
225 for (
unsigned int i=0; i<8; i++)
228 double c1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
229 double s1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
230 double c3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
231 double s3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
232 double c23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
233 double s23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
234 double c4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
235 double s4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
236 double c5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
237 double s5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
238 double c6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
239 double s6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
246 for (
unsigned int i=0; i< 8; i++)
249 double px = fMw[0][3];
250 double py = fMw[1][3];
251 double pz = fMw[2][3];
254 double a_2 = px*px+py*py;
256 if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) {
259 c1[4] = cos(q[0]+M_PI);
260 s1[4] = sin(q[0]+M_PI);
263 double a = sqrt(a_2);
271 for(
unsigned int i=0;i<8;i+=4) {
272 q_sol[i][0] = atan2(s1[i],c1[i]);
273 if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod) ==
true) {
274 q_sol[i][0] = q1_mod;
275 for(
unsigned int j=1;j<4;j++) {
278 q_sol[i+j][0] = q_sol[i][0];
282 for(
unsigned int j=1;j<4;j++)
294 for(
unsigned int i=0; i<8; i+=4) {
297 - 2*(
a1*c1[i]*px +
a1*s1[i]*py +
d1*pz)) / (2*
a2);
298 q_sol[i][2] = atan2(
a3,
d4) + atan2(K, sqrt(
d4*
d4+
a3*
a3-K*K));
299 q_sol[i+2][2] = atan2(
a3,
d4) + atan2(K, -sqrt(
d4*
d4+
a3*
a3-K*K));
301 for (
unsigned int j=0; j<4; j+=2) {
302 if (convertJointPositionInLimits(2, q_sol[i+j][2], q3_mod) ==
true) {
303 for(
unsigned int k=0; k<2; k++) {
304 q_sol[i+j+k][2] = q3_mod;
305 c3[i+j+k] = cos(q3_mod);
306 s3[i+j+k] = sin(q3_mod);
310 for(
unsigned int k=0; k<2; k++)
322 double q23[8], q2_mod;
323 for (
unsigned int i=0; i<8; i+=2) {
326 c23[i] = (-(
a3-
a2*c3[i])*(c1[i]*px+s1[i]*py-
a1)-(
d1-pz)*(
d4+
a2*s3[i]))
327 / ( (c1[i]*px+s1[i]*py-
a1)*(c1[i]*px+s1[i]*py-
a1) +(
d1-pz)*(
d1-pz) );
328 s23[i] = ((
d4+
a2*s3[i])*(c1[i]*px+s1[i]*py-
a1)-(
d1-pz)*(
a3-
a2*c3[i]))
329 / ( (c1[i]*px+s1[i]*py-
a1)*(c1[i]*px+s1[i]*py-
a1) +(
d1-pz)*(
d1-pz) );
330 q23[i] = atan2(s23[i],c23[i]);
333 q_sol[i][1] = q23[i] - q_sol[i][2];
335 if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod) ==
true) {
336 for(
unsigned int j=0; j<2; j++) {
337 q_sol[i+j][1] = q2_mod;
343 for(
unsigned int j=0; j<2; j++)
354 double r13 = fMw[0][2];
355 double r23 = fMw[1][2];
356 double r33 = fMw[2][2];
357 double s4s5, c4s5, q4_mod, q5_mod;
358 for (
unsigned int i=0; i<8; i+=2) {
360 s4s5 = -s1[i]*r13+c1[i]*r23;
361 c4s5 = c1[i]*c23[i]*r13+s1[i]*c23[i]*r23-s23[i]*r33;
364 c5[i] = c1[i]*s23[i]*r13+s1[i]*s23[i]*r23+c23[i]*r33;
371 if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod) ==
true) {
372 for(
unsigned int j=0; j<2; j++) {
373 q_sol[i+j][3] = q[3];
374 q_sol[i+j][4] = q5_mod;
375 c4[i] = cos(q_sol[i+j][3]);
376 s4[i] = sin(q_sol[i+j][3]);
380 for(
unsigned int j=0; j<2; j++)
387 if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
391 q_sol[i][3] = atan2(s4s5, c4s5);
394 q_sol[i][3] = atan2(s4s5, c4s5);
396 if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod) ==
true) {
397 q_sol[i][3] = q4_mod;
404 if (q_sol[i][3] > 0.)
405 q_sol[i+1][3] = q_sol[i][3] + M_PI;
407 q_sol[i+1][3] = q_sol[i][3] - M_PI;
408 if (convertJointPositionInLimits(3, q_sol[i+1][3], q4_mod) ==
true) {
409 q_sol[i+1][3] = q4_mod;
410 c4[i+1] = cos(q4_mod);
411 s4[i+1] = sin(q4_mod);
418 for (
unsigned int j=0; j<2; j++) {
419 if (ok[i+j] ==
true) {
420 c5[i+j] = c1[i+j]*s23[i+j]*r13+s1[i+j]*s23[i+j]*r23+c23[i+j]*r33;
421 s5[i+j] = (c1[i+j]*c23[i+j]*c4[i+j]-s1[i+j]*s4[i+j])*r13
422 +(s1[i+j]*c23[i+j]*c4[i+j]+c1[i+j]*s4[i+j])*r23-s23[i+j]*c4[i+j]*r33;
424 q_sol[i+j][4] = atan2(s5[i+j], c5[i+j]);
425 if (convertJointPositionInLimits(4, q_sol[i+j][4], q5_mod) ==
true) {
426 q_sol[i+j][4] = q5_mod;
440 double r12 = fMw[0][1];
441 double r22 = fMw[1][1];
442 double r32 = fMw[2][1];
444 for (
unsigned int i=0; i<8; i++) {
445 c6[i] = -(c1[i]*c23[i]*s4[i]+s1[i]*c4[i])*r12
446 +(c1[i]*c4[i]-s1[i]*c23[i]*s4[i])*r22+s23[i]*s4[i]*r32;
447 s6[i] = -(c1[i]*c23[i]*c4[i]*c5[i]-c1[i]*s23[i]*s5[i]
448 -s1[i]*s4[i]*c5[i])*r12
449 -(s1[i]*c23[i]*c4[i]*c5[i]-s1[i]*s23[i]*s5[i]+c1[i]*s4[i]*c5[i])*r22
450 +(c23[i]*s5[i]+s23[i]*c4[i]*c5[i])*r32;
452 q_sol[i][5] = atan2(s6[i], c6[i]);
453 if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod) ==
true) {
454 q_sol[i][5] = q6_mod;
462 unsigned int nbsol = 0;
463 unsigned int sol = 0;
465 for (
unsigned int i=0; i<8; i++) {
473 weight[1] = weight[2] = 4;
475 for (
unsigned int j=0; j< 6; j++) {
476 double rought_dist = q[j]- q_sol[i][j];
477 double modulo_dist = rought_dist;
478 if (rought_dist > 0) {
479 if (fabs(rought_dist - 2*M_PI) < fabs(rought_dist))
480 modulo_dist = rought_dist - 2*M_PI;
483 if (fabs(rought_dist + 2*M_PI) < fabs(rought_dist))
484 modulo_dist = rought_dist + 2*M_PI;
494 for (
unsigned int i=0; i<8; i++) {
496 if (dist[i] < dist[sol]) sol = i;
636 fMc = fMe * this->
eMc;
744 double c23 = cos(q2+q3);
745 double s23 = sin(q2+q3);
747 fMe[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
748 fMe[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
749 fMe[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
751 fMe[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
752 fMe[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
753 fMe[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
755 fMe[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
756 fMe[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
757 fMe[2][2] = -s23*c4*s5+c23*c5;
759 fMe[0][3] = c1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+
a1+
a2*c2)-s1*s4*s5*
d6;
760 fMe[1][3] = s1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+
a1+
a2*c2)+c1*s4*s5*
d6;
840 double c23 = cos(q2+q3);
841 double s23 = sin(q2+q3);
844 fMw[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
845 fMw[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
846 fMw[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
848 fMw[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
849 fMw[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
850 fMw[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
852 fMw[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
853 fMw[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
854 fMw[2][2] = -s23*c4*s5+c23*c5;
856 fMw[0][3] = c1*(-c23*
a3+s23*
d4+
a1+
a2*c2);
857 fMw[1][3] = s1*(-c23*
a3+s23*
d4+
a1+
a2*c2);
979 for (
unsigned int i=0; i<3; i++ ) {
980 for (
unsigned int j=0; j<3; j++ ) {
981 V[i][j] = V[i+3][j+3] = wRf[i][j];
992 for (
unsigned int i=0; i<3; i++ ) {
993 for (
unsigned int j=0; j<3; j++ ) {
994 V[i][j+3] = block2[i][j];
1062 double c1 = cos(q1);
1063 double s1 = sin(q1);
1064 double c2 = cos(q2);
1065 double s2 = sin(q2);
1066 double c3 = cos(q3);
1067 double s3 = sin(q3);
1068 double c4 = cos(q4);
1069 double s4 = sin(q4);
1070 double c5 = cos(q5);
1071 double s5 = sin(q5);
1072 double c23 = cos(q2+q3);
1073 double s23 = sin(q2+q3);
1083 J1[0] = -s1*(-c23*
a3+s23*
d4+
a1+
a2*c2);
1084 J1[1] = c1*(-c23*
a3+s23*
d4+
a1+
a2*c2);
1090 J2[0] = c1*(s23*
a3+c23*
d4-
a2*s2);
1091 J2[1] = s1*(s23*
a3+c23*
d4-
a2*s2);
1092 J2[2] = c23*
a3-s23*
d4-
a2*c2;
1097 J3[0] = c1*(
a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*
d4);
1098 J3[1] = s1*(
a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*
d4);
1099 J3[2] = -
a3*(s2*s3-c2*c3)-
d4*(s2*c3+c2*s3);
1114 J5[3] = -c23*c1*s4-s1*c4;
1115 J5[4] = c1*c4-c23*s1*s4;
1121 J6[3] = (c1*c23*c4-s1*s4)*s5+c1*s23*c5;
1122 J6[4] = (s1*c23*c4+c1*s4)*s5+s1*s23*c5;
1123 J6[5] = -s23*c4*s5+c23*c5;
1126 for (
unsigned int i=0;i<6;i++) {
1165 for (
unsigned int i=0; i<6; i++ )
1179 vpMatrix block2 = (fRw*etw).skew();
1181 for (
unsigned int i=0; i<3; i++ )
1182 for (
unsigned int j=0; j<3; j++ )
1183 V[i][j+3] = block2[i][j];
1260 <<
"Joint Max (deg):" << std::endl
1261 <<
"\t" << jmax.
t() << std::endl
1263 <<
"Joint Min (deg): " << std::endl
1264 <<
"\t" << jmin.
t() << std::endl
1266 <<
"Coupling 5-6:" << std::endl
1267 <<
"\t" << viper.
c56 << std::endl
1269 <<
"eMc: "<< std::endl
1270 <<
"\tTranslation (m): "
1271 << viper.
eMc[0][3] <<
" "
1272 << viper.
eMc[1][3] <<
" "
1274 <<
"\t" << std::endl
1275 <<
"\tRotation Rxyz (rad) : "
1279 <<
"\t" << std::endl
1280 <<
"\tRotation Rxyz (deg) : "
1284 <<
"\t" << std::endl;
Definition of the vpMatrix class.
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
vpHomogeneousMatrix eMc
End effector to camera transformation.
vpRotationMatrix inverse() const
invert the rotation matrix
Modelisation of the ADEPT Viper robot.
unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void get_eJe(const vpColVector &q, vpMatrix &eJe)
void setIdentity()
Basic initialisation (identity)
vpColVector getJointMin()
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q)
The vpRotationMatrix considers the particular case of a rotation matrix.
double c56
Mechanical coupling between joint 5 and joint 6.
void get_eMc(vpHomogeneousMatrix &eMc)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
vpRowVector t() const
transpose of Vector
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe)
void extract(vpRotationMatrix &R) const
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpImagePoint &ip)
static double rad(double deg)
void get_fJe(const vpColVector &q, vpMatrix &fJe)
vpColVector getJointMax()
static double deg(double rad)
void get_wMe(vpHomogeneousMatrix &wMe)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
vpHomogeneousMatrix inverse() const
void get_fJw(const vpColVector &q, vpMatrix &fJw)
void get_cMe(vpHomogeneousMatrix &cMe)
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
void get_cVe(vpVelocityTwistMatrix &cVe)
unsigned int getRows() const
Return the number of rows of the matrix.
Class that consider the case of a translation vector.
static const unsigned int njoint
Number of joint.
vpHomogeneousMatrix get_fMc(const vpColVector &q)
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q)
void resize(const unsigned int i, const bool flagNullify=true)
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw)