48 #include <visp3/core/vpDebug.h> 49 #include <visp3/core/vpVelocityTwistMatrix.h> 50 #include <visp3/robot/vpRobotException.h> 51 #include <visp3/core/vpCameraParameters.h> 52 #include <visp3/core/vpRxyzVector.h> 53 #include <visp3/core/vpTranslationVector.h> 54 #include <visp3/core/vpRotationMatrix.h> 55 #include <visp3/robot/vpViper.h> 67 : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), d7(0), c56(0), joint_max(), joint_min()
79 c56 = -341.33 / 9102.22;
149 vpViper::convertJointPositionInLimits(
unsigned int joint,
const double &q,
double &q_mod,
const bool &verbose)
const 168 std::cout <<
"Joint " << joint <<
" not in limits: " 169 << this->
joint_min[joint] <<
" < " << q <<
" < " << this->
joint_max[joint] << std::endl;
233 for (
unsigned int i=0; i<8; i++)
236 double c1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
237 double s1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
238 double c3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
239 double s3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
240 double c23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
241 double s23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
242 double c4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
243 double s4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
244 double c5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
245 double s5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
246 double c6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
247 double s6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
254 for (
unsigned int i=0; i< 8; i++)
257 double px = fMw[0][3];
258 double py = fMw[1][3];
259 double pz = fMw[2][3];
262 double a_2 = px*px+py*py;
264 if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) {
267 c1[4] = cos(q[0]+M_PI);
268 s1[4] = sin(q[0]+M_PI);
271 double a = sqrt(a_2);
279 for(
unsigned int i=0;i<8;i+=4) {
280 q_sol[i][0] = atan2(s1[i],c1[i]);
281 if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) ==
true) {
282 q_sol[i][0] = q1_mod;
283 for(
unsigned int j=1;j<4;j++) {
286 q_sol[i+j][0] = q_sol[i][0];
290 for(
unsigned int j=1;j<4;j++)
297 for(
unsigned int i=0; i<8; i+=4) {
300 - 2*(
a1*c1[i]*px +
a1*s1[i]*py +
d1*pz)) / (2*
a2);
303 q_sol[i][2] = atan2(
a3,
d4) + atan2(K, sqrt(d4_a3_K));
304 q_sol[i+2][2] = atan2(
a3,
d4) + atan2(K, -sqrt(d4_a3_K));
306 for (
unsigned int j=0; j<4; j+=2) {
308 for(
unsigned int k=0; k<2; k++)
312 if (convertJointPositionInLimits(2, q_sol[i+j][2], q3_mod, verbose) ==
true) {
313 for(
unsigned int k=0; k<2; k++) {
314 q_sol[i+j+k][2] = q3_mod;
315 c3[i+j+k] = cos(q3_mod);
316 s3[i+j+k] = sin(q3_mod);
320 for(
unsigned int k=0; k<2; k++)
333 double q23[8], q2_mod;
334 for (
unsigned int i=0; i<8; i+=2) {
337 c23[i] = (-(
a3-
a2*c3[i])*(c1[i]*px+s1[i]*py-
a1)-(
d1-pz)*(
d4+
a2*s3[i]))
338 / ( (c1[i]*px+s1[i]*py-
a1)*(c1[i]*px+s1[i]*py-
a1) +(
d1-pz)*(
d1-pz) );
339 s23[i] = ((
d4+
a2*s3[i])*(c1[i]*px+s1[i]*py-
a1)-(
d1-pz)*(
a3-
a2*c3[i]))
340 / ( (c1[i]*px+s1[i]*py-
a1)*(c1[i]*px+s1[i]*py-
a1) +(
d1-pz)*(
d1-pz) );
341 q23[i] = atan2(s23[i],c23[i]);
344 q_sol[i][1] = q23[i] - q_sol[i][2];
346 if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) ==
true) {
347 for(
unsigned int j=0; j<2; j++) {
348 q_sol[i+j][1] = q2_mod;
354 for(
unsigned int j=0; j<2; j++)
365 double r13 = fMw[0][2];
366 double r23 = fMw[1][2];
367 double r33 = fMw[2][2];
368 double s4s5, c4s5, q4_mod, q5_mod;
369 for (
unsigned int i=0; i<8; i+=2) {
371 s4s5 = -s1[i]*r13+c1[i]*r23;
372 c4s5 = c1[i]*c23[i]*r13+s1[i]*c23[i]*r23-s23[i]*r33;
375 c5[i] = c1[i]*s23[i]*r13+s1[i]*s23[i]*r23+c23[i]*r33;
382 if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) ==
true) {
383 for(
unsigned int j=0; j<2; j++) {
384 q_sol[i+j][3] = q[3];
385 q_sol[i+j][4] = q5_mod;
386 c4[i] = cos(q_sol[i+j][3]);
387 s4[i] = sin(q_sol[i+j][3]);
391 for(
unsigned int j=0; j<2; j++)
397 #if 0 // Modified 2016/03/10 since if and else are the same 399 if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
403 q_sol[i][3] = atan2(s4s5, c4s5);
406 q_sol[i][3] = atan2(s4s5, c4s5);
409 q_sol[i][3] = atan2(s4s5, c4s5);
411 if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) ==
true) {
412 q_sol[i][3] = q4_mod;
419 if (q_sol[i][3] > 0.)
420 q_sol[i+1][3] = q_sol[i][3] + M_PI;
422 q_sol[i+1][3] = q_sol[i][3] - M_PI;
423 if (convertJointPositionInLimits(3, q_sol[i+1][3], q4_mod, verbose) ==
true) {
424 q_sol[i+1][3] = q4_mod;
425 c4[i+1] = cos(q4_mod);
426 s4[i+1] = sin(q4_mod);
433 for (
unsigned int j=0; j<2; j++) {
434 if (ok[i+j] ==
true) {
435 c5[i+j] = c1[i+j]*s23[i+j]*r13+s1[i+j]*s23[i+j]*r23+c23[i+j]*r33;
436 s5[i+j] = (c1[i+j]*c23[i+j]*c4[i+j]-s1[i+j]*s4[i+j])*r13
437 +(s1[i+j]*c23[i+j]*c4[i+j]+c1[i+j]*s4[i+j])*r23-s23[i+j]*c4[i+j]*r33;
439 q_sol[i+j][4] = atan2(s5[i+j], c5[i+j]);
440 if (convertJointPositionInLimits(4, q_sol[i+j][4], q5_mod, verbose) ==
true) {
441 q_sol[i+j][4] = q5_mod;
455 double r12 = fMw[0][1];
456 double r22 = fMw[1][1];
457 double r32 = fMw[2][1];
459 for (
unsigned int i=0; i<8; i++) {
460 c6[i] = -(c1[i]*c23[i]*s4[i]+s1[i]*c4[i])*r12
461 +(c1[i]*c4[i]-s1[i]*c23[i]*s4[i])*r22+s23[i]*s4[i]*r32;
462 s6[i] = -(c1[i]*c23[i]*c4[i]*c5[i]-c1[i]*s23[i]*s5[i]
463 -s1[i]*s4[i]*c5[i])*r12
464 -(s1[i]*c23[i]*c4[i]*c5[i]-s1[i]*s23[i]*s5[i]+c1[i]*s4[i]*c5[i])*r22
465 +(c23[i]*s5[i]+s23[i]*c4[i]*c5[i])*r32;
467 q_sol[i][5] = atan2(s6[i], c6[i]);
468 if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) ==
true) {
469 q_sol[i][5] = q6_mod;
477 unsigned int nbsol = 0;
478 unsigned int sol = 0;
480 for (
unsigned int i=0; i<8; i++) {
488 weight[1] = weight[2] = 4;
490 for (
unsigned int j=0; j< 6; j++) {
491 double rought_dist = q[j]- q_sol[i][j];
492 double modulo_dist = rought_dist;
493 if (rought_dist > 0) {
494 if (fabs(rought_dist - 2*M_PI) < fabs(rought_dist))
495 modulo_dist = rought_dist - 2*M_PI;
498 if (fabs(rought_dist + 2*M_PI) < fabs(rought_dist))
499 modulo_dist = rought_dist + 2*M_PI;
509 for (
unsigned int i=0; i<8; i++) {
511 if (dist[i] < dist[sol]) sol = i;
655 fMc = fMe * this->
eMc;
763 double c23 = cos(q2+q3);
764 double s23 = sin(q2+q3);
766 fMe[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
767 fMe[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
768 fMe[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
770 fMe[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
771 fMe[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
772 fMe[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
774 fMe[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
775 fMe[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
776 fMe[2][2] = -s23*c4*s5+c23*c5;
778 fMe[0][3] = c1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+
a1+
a2*c2)-s1*s4*s5*
d6;
779 fMe[1][3] = s1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+
a1+
a2*c2)+c1*s4*s5*
d6;
859 double c23 = cos(q2+q3);
860 double s23 = sin(q2+q3);
862 fMw[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
863 fMw[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
864 fMw[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
866 fMw[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
867 fMw[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
868 fMw[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
870 fMw[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
871 fMw[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
872 fMw[2][2] = -s23*c4*s5+c23*c5;
874 fMw[0][3] = c1*(-c23*
a3+s23*
d4+
a1+
a2*c2);
875 fMw[1][3] = s1*(-c23*
a3+s23*
d4+
a1+
a2*c2);
1013 for (
unsigned int i=0; i<3; i++ ) {
1014 for (
unsigned int j=0; j<3; j++ ) {
1015 V[i][j] = V[i+3][j+3] = wRf[i][j];
1026 for (
unsigned int i=0; i<3; i++ ) {
1027 for (
unsigned int j=0; j<3; j++ ) {
1028 V[i][j+3] = block2[i][j];
1096 double c1 = cos(q1);
1097 double s1 = sin(q1);
1098 double c2 = cos(q2);
1099 double s2 = sin(q2);
1100 double c3 = cos(q3);
1101 double s3 = sin(q3);
1102 double c4 = cos(q4);
1103 double s4 = sin(q4);
1104 double c5 = cos(q5);
1105 double s5 = sin(q5);
1106 double c23 = cos(q2+q3);
1107 double s23 = sin(q2+q3);
1117 J1[0] = -s1*(-c23*
a3+s23*
d4+
a1+
a2*c2);
1118 J1[1] = c1*(-c23*
a3+s23*
d4+
a1+
a2*c2);
1124 J2[0] = c1*(s23*
a3+c23*
d4-
a2*s2);
1125 J2[1] = s1*(s23*
a3+c23*
d4-
a2*s2);
1126 J2[2] = c23*
a3-s23*
d4-
a2*c2;
1131 J3[0] = c1*(
a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*
d4);
1132 J3[1] = s1*(
a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*
d4);
1133 J3[2] = -
a3*(s2*s3-c2*c3)-
d4*(s2*c3+c2*s3);
1148 J5[3] = -c23*c1*s4-s1*c4;
1149 J5[4] = c1*c4-c23*s1*s4;
1155 J6[3] = (c1*c23*c4-s1*s4)*s5+c1*s23*c5;
1156 J6[4] = (s1*c23*c4+c1*s4)*s5+s1*s23*c5;
1157 J6[5] = -s23*c4*s5+c23*c5;
1160 for (
unsigned int i=0;i<6;i++) {
1199 for (
unsigned int i=0; i<6; i++ )
1213 vpMatrix block2 = (fRw*etw).skew();
1215 for (
unsigned int i=0; i<3; i++ )
1216 for (
unsigned int j=0; j<3; j++ )
1217 V[i][j+3] = block2[i][j];
1328 <<
"Joint Max (deg):" << std::endl
1329 <<
"\t" << jmax.
t() << std::endl
1331 <<
"Joint Min (deg): " << std::endl
1332 <<
"\t" << jmin.
t() << std::endl
1334 <<
"Coupling 5-6:" << std::endl
1335 <<
"\t" << viper.
c56 << std::endl
1337 <<
"eMc: "<< std::endl
1338 <<
"\tTranslation (m): " 1339 << viper.
eMc[0][3] <<
" " 1340 << viper.
eMc[1][3] <<
" " 1342 <<
"\t" << std::endl
1343 <<
"\tRotation Rxyz (rad) : " 1347 <<
"\t" << std::endl
1348 <<
"\tRotation Rxyz (deg) : " 1352 <<
"\t" << std::endl;
Implementation of a matrix and operations on matrices.
double d7
for force/torque location
vpRxyzVector buildFrom(const vpRotationMatrix &R)
vpHomogeneousMatrix eMc
End effector to camera transformation.
Modelisation of the ADEPT Viper robot.
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
void get_eMs(vpHomogeneousMatrix &eMs) 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.
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpViper &viper)
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
vpColVector getJointMin() const
void get_wMe(vpHomogeneousMatrix &wMe) const
unsigned int getRows() const
Return the number of rows of the 2D array.
vpHomogeneousMatrix inverse() const
unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q, const bool &verbose=false) const
void get_eMc(vpHomogeneousMatrix &eMc) const
void extract(vpRotationMatrix &R) const
vpRotationMatrix inverse() const
Implementation of a rotation matrix and operations on such kind of matrices.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
double c56
Mechanical coupling between joint 5 and joint 6.
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
void get_cMe(vpHomogeneousMatrix &cMe) const
Implementation of a velocity twist matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
double getCoupl56() const
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
static double deg(double rad)
Implementation of column vector and the associated operations.
void get_cVe(vpVelocityTwistMatrix &cVe) const
Implementation of a rotation vector as Euler angle minimal representation.
void get_fJw(const vpColVector &q, vpMatrix &fJw) const
Class that consider the case of a translation vector.
static const unsigned int njoint
Number of joint.
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
vpColVector getJointMax() const
void resize(const unsigned int i, const bool flagNullify=true)