ViSP  2.10.0
vpViper.cpp
1 /****************************************************************************
2  *
3  * $Id: vpViper.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface for a generic ADEPT Viper (either 650 or 850) robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
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>
60 #include <cmath> // std::fabs
61 #include <limits> // numeric_limits
62 
63 const unsigned int vpViper::njoint = 6;
64 
71  : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), c56(0), joint_max(), joint_min()
72 {
73  // Default values are initialized
74 
75  // Denavit Hartenberg parameters
76  a1 = 0.075;
77  a2 = 0.365;
78  a3 = 0.090;
79  d1 = 0.335;
80  d4 = 0.405;
81  d6 = 0.080;
82  c56 = -341.33 / 9102.22;
83 
84  // Software joint limits in radians
86  joint_min[0] = vpMath::rad(-170);
87  joint_min[1] = vpMath::rad(-190);
88  joint_min[2] = vpMath::rad(-29);
89  joint_min[3] = vpMath::rad(-190);
90  joint_min[4] = vpMath::rad(-120);
91  joint_min[5] = vpMath::rad(-360);
93  joint_max[0] = vpMath::rad(170);
94  joint_max[1] = vpMath::rad(45);
95  joint_max[2] = vpMath::rad(256);
96  joint_max[3] = vpMath::rad(190);
97  joint_max[4] = vpMath::rad(120);
98  joint_max[5] = vpMath::rad(360);
99 
100  // End effector to camera transformation
101  eMc.setIdentity();
102 }
103 
104 
105 
106 
130 {
132  fMc = get_fMc(q);
133 
134  return fMc;
135 }
136 
151 bool
152 vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod, const bool &verbose) const
153 {
154  double eps = 0.01;
155  if (q >= joint_min[joint]-eps && q <= joint_max[joint]+eps ) {
156  q_mod = q;
157  return true;
158  }
159 
160  q_mod = q + 2*M_PI;
161  if (q_mod >= joint_min[joint]-eps && q_mod <= joint_max[joint]+eps ) {
162  return true;
163  }
164 
165  q_mod = q - 2*M_PI;
166  if (q_mod >= joint_min[joint]-eps && q_mod <= joint_max[joint]+eps ) {
167  return true;
168  }
169 
170  if (verbose) {
171  std::cout << "Joint " << joint << " not in limits: "
172  << this->joint_min[joint] << " < " << q << " < " << this->joint_max[joint] << std::endl;
173 
174  }
175 
176  return false;
177 }
178 
231 unsigned int
232 vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector & q, const bool &verbose) const
233 {
234  vpColVector q_sol[8];
235 
236  for (unsigned int i=0; i<8; i++)
237  q_sol[i].resize(6);
238 
239  double c1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
240  double s1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
241  double c3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
242  double s3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
243  double c23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
244  double s23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
245  double c4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
246  double s4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
247  double c5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
248  double s5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
249  double c6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
250  double s6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
251 
252  bool ok[8];
253 
254  if (q.getRows() != njoint)
255  q.resize(6);
256 
257  for (unsigned int i=0; i< 8; i++)
258  ok[i] = true;
259 
260  double px = fMw[0][3]; // a*c1
261  double py = fMw[1][3]; // a*s1
262  double pz = fMw[2][3];
263 
264  // Compute q1
265  double a_2 = px*px+py*py;
266  //if (a_2 == 0) {// singularity
267  if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) {// singularity
268  c1[0] = cos(q[0]);
269  s1[0] = sin(q[0]);
270  c1[4] = cos(q[0]+M_PI);
271  s1[4] = sin(q[0]+M_PI);
272  }
273  else {
274  double a = sqrt(a_2);
275  c1[0] = px/a;
276  s1[0] = py/a;
277  c1[4] = -px/a;
278  s1[4] = -py/a;
279  }
280 
281  double q1_mod;
282  for(unsigned int i=0;i<8;i+=4) {
283  q_sol[i][0] = atan2(s1[i],c1[i]);
284  if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) == true) {
285  q_sol[i][0] = q1_mod;
286  for(unsigned int j=1;j<4;j++) {
287  c1[i+j] = c1[i];
288  s1[i+j] = s1[i];
289  q_sol[i+j][0] = q_sol[i][0];
290  }
291  }
292  else {
293  for(unsigned int j=1;j<4;j++)
294  ok[i+j] = false;
295  }
296  }
297 
298  // Compute q3
299  double K, q3_mod;
300  for(unsigned int i=0; i<8; i+=4) {
301  if(ok[i] == true) {
302  K = (px*px+py*py+pz*pz+a1*a1-a2*a2-a3*a3+d1*d1-d4*d4
303  - 2*(a1*c1[i]*px + a1*s1[i]*py + d1*pz)) / (2*a2);
304  double d4_a3_K = d4*d4+a3*a3-K*K;
305 
306  q_sol[i][2] = atan2(a3, d4) + atan2(K, sqrt(d4_a3_K));
307  q_sol[i+2][2] = atan2(a3, d4) + atan2(K, -sqrt(d4_a3_K));
308 
309  for (unsigned int j=0; j<4; j+=2) {
310  if (d4_a3_K < 0) {
311  for(unsigned int k=0; k<2; k++)
312  ok[i+j+k] = false;
313  }
314  else {
315  if (convertJointPositionInLimits(2, q_sol[i+j][2], q3_mod, verbose) == true) {
316  for(unsigned int k=0; k<2; k++) {
317  q_sol[i+j+k][2] = q3_mod;
318  c3[i+j+k] = cos(q3_mod);
319  s3[i+j+k] = sin(q3_mod);
320  }
321  }
322  else {
323  for(unsigned int k=0; k<2; k++)
324  ok[i+j+k] = false;
325  }
326  }
327  }
328  }
329  }
330  // std::cout << "ok apres q3: ";
331  // for (unsigned int i=0; i< 8; i++)
332  // std::cout << ok[i] << " ";
333  // std::cout << std::endl;
334 
335  // Compute q2
336  double q23[8], q2_mod;
337  for (unsigned int i=0; i<8; i+=2) {
338  if (ok[i] == true) {
339  // Compute q23 = q2+q3
340  c23[i] = (-(a3-a2*c3[i])*(c1[i]*px+s1[i]*py-a1)-(d1-pz)*(d4+a2*s3[i]))
341  / ( (c1[i]*px+s1[i]*py-a1)*(c1[i]*px+s1[i]*py-a1) +(d1-pz)*(d1-pz) );
342  s23[i] = ((d4+a2*s3[i])*(c1[i]*px+s1[i]*py-a1)-(d1-pz)*(a3-a2*c3[i]))
343  / ( (c1[i]*px+s1[i]*py-a1)*(c1[i]*px+s1[i]*py-a1) +(d1-pz)*(d1-pz) );
344  q23[i] = atan2(s23[i],c23[i]);
345  //std::cout << i << " c23 = " << c23[i] << " s23 = " << s23[i] << std::endl;
346  // q2 = q23 - q3
347  q_sol[i][1] = q23[i] - q_sol[i][2];
348 
349  if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) == true) {
350  for(unsigned int j=0; j<2; j++) {
351  q_sol[i+j][1] = q2_mod;
352  c23[i+j] = c23[i];
353  s23[i+j] = s23[i];
354  }
355  }
356  else {
357  for(unsigned int j=0; j<2; j++)
358  ok[i+j] = false;
359  }
360  }
361  }
362  // std::cout << "ok apres q2: ";
363  // for (unsigned int i=0; i< 8; i++)
364  // std::cout << ok[i] << " ";
365  // std::cout << std::endl;
366 
367  // Compute q4 as long as s5 != 0
368  double r13 = fMw[0][2];
369  double r23 = fMw[1][2];
370  double r33 = fMw[2][2];
371  double s4s5, c4s5, q4_mod, q5_mod;
372  for (unsigned int i=0; i<8; i+=2) {
373  if (ok[i] == true) {
374  s4s5 = -s1[i]*r13+c1[i]*r23;
375  c4s5 = c1[i]*c23[i]*r13+s1[i]*c23[i]*r23-s23[i]*r33;
376  if (fabs(s4s5) < vpMath::rad(0.5) && fabs(c4s5) < vpMath::rad(0.5)) {
377  // s5 = 0
378  c5[i] = c1[i]*s23[i]*r13+s1[i]*s23[i]*r23+c23[i]*r33;
379  //std::cout << "Singularity: s5 near 0: ";
380  if (c5[i] > 0.)
381  q_sol[i][4] = 0.0;
382  else
383  q_sol[i][4] = M_PI;
384 
385  if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) == true) {
386  for(unsigned int j=0; j<2; j++) {
387  q_sol[i+j][3] = q[3]; // keep current q4
388  q_sol[i+j][4] = q5_mod;
389  c4[i] = cos(q_sol[i+j][3]);
390  s4[i] = sin(q_sol[i+j][3]);
391  }
392  }
393  else {
394  for(unsigned int j=0; j<2; j++)
395  ok[i+j] = false;
396  }
397  }
398  else {
399  // s5 != 0
400  //if (c4s5 == 0) {
401  if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
402  // c4 = 0
403  // vpTRACE("c4 = 0");
404  // q_sol[i][3] = q[3]; // keep current position
405  q_sol[i][3] = atan2(s4s5, c4s5);
406  }
407  else {
408  q_sol[i][3] = atan2(s4s5, c4s5);
409  }
410  if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) == true) {
411  q_sol[i][3] = q4_mod;
412  c4[i] = cos(q4_mod);
413  s4[i] = sin(q4_mod);
414  }
415  else {
416  ok[i] = false;
417  }
418  if (q_sol[i][3] > 0.)
419  q_sol[i+1][3] = q_sol[i][3] + M_PI;
420  else
421  q_sol[i+1][3] = q_sol[i][3] - M_PI;
422  if (convertJointPositionInLimits(3, q_sol[i+1][3], q4_mod, verbose) == true) {
423  q_sol[i+1][3] = q4_mod;
424  c4[i+1] = cos(q4_mod);
425  s4[i+1] = sin(q4_mod);
426  }
427  else {
428  ok[i+1] = false;
429  }
430 
431  // Compute q5
432  for (unsigned int j=0; j<2; j++) {
433  if (ok[i+j] == true) {
434  c5[i+j] = c1[i+j]*s23[i+j]*r13+s1[i+j]*s23[i+j]*r23+c23[i+j]*r33;
435  s5[i+j] = (c1[i+j]*c23[i+j]*c4[i+j]-s1[i+j]*s4[i+j])*r13
436  +(s1[i+j]*c23[i+j]*c4[i+j]+c1[i+j]*s4[i+j])*r23-s23[i+j]*c4[i+j]*r33;
437 
438  q_sol[i+j][4] = atan2(s5[i+j], c5[i+j]);
439  if (convertJointPositionInLimits(4, q_sol[i+j][4], q5_mod, verbose) == true) {
440  q_sol[i+j][4] = q5_mod;
441  }
442  else {
443 
444  ok[i+j] = false;
445  }
446  }
447  }
448  }
449  }
450  }
451 
452  // Compute q6
453  // 4 solutions for q6 and 4 more solutions by flipping the wrist (see below)
454  double r12 = fMw[0][1];
455  double r22 = fMw[1][1];
456  double r32 = fMw[2][1];
457  double q6_mod;
458  for (unsigned int i=0; i<8; i++) {
459  c6[i] = -(c1[i]*c23[i]*s4[i]+s1[i]*c4[i])*r12
460  +(c1[i]*c4[i]-s1[i]*c23[i]*s4[i])*r22+s23[i]*s4[i]*r32;
461  s6[i] = -(c1[i]*c23[i]*c4[i]*c5[i]-c1[i]*s23[i]*s5[i]
462  -s1[i]*s4[i]*c5[i])*r12
463  -(s1[i]*c23[i]*c4[i]*c5[i]-s1[i]*s23[i]*s5[i]+c1[i]*s4[i]*c5[i])*r22
464  +(c23[i]*s5[i]+s23[i]*c4[i]*c5[i])*r32;
465 
466  q_sol[i][5] = atan2(s6[i], c6[i]);
467  if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) == true) {
468  q_sol[i][5] = q6_mod;
469  }
470  else {
471  ok[i] = false;
472  }
473  }
474 
475  // Select the best config in terms of distance from the current position
476  unsigned int nbsol = 0;
477  unsigned int sol = 0;
478  vpColVector dist(8);
479  for (unsigned int i=0; i<8; i++) {
480  if (ok[i] == true) {
481  nbsol ++;
482  sol = i;
483  // dist[i] = vpColVector::distance(q, q_sol[i]);
484  vpColVector weight(6);
485  weight = 1;
486  weight[0] = 8;
487  weight[1] = weight[2] = 4;
488  dist[i] = 0;
489  for (unsigned int j=0; j< 6; j++) {
490  double rought_dist = q[j]- q_sol[i][j];
491  double modulo_dist = rought_dist;
492  if (rought_dist > 0) {
493  if (fabs(rought_dist - 2*M_PI) < fabs(rought_dist))
494  modulo_dist = rought_dist - 2*M_PI;
495  }
496  else {
497  if (fabs(rought_dist + 2*M_PI) < fabs(rought_dist))
498  modulo_dist = rought_dist + 2*M_PI;
499  }
500  //std::cout << "dist " << i << ": " << rought_dist << " modulo: " << modulo_dist << std::endl;
501  dist[i] += weight[j]*vpMath::sqr(modulo_dist);
502  }
503  }
504  // std::cout << "sol " << i << " [" << ok[i] << "] dist: " << dist[i] << " q: " << q_sol[i].t() << std::endl;
505  }
506  //std::cout << "dist: " << dist.t() << std::endl;
507  if (nbsol) {
508  for (unsigned int i=0; i<8; i++) {
509  if (ok[i] == true)
510  if (dist[i] < dist[sol]) sol = i;
511  }
512  // Update the inverse kinematics solution
513  q = q_sol[sol];
514 
515  // std::cout << "Nearest solution (" << sol << ") with distance ("
516  // << dist[sol] << "): " << q_sol[sol].t() << std::endl;
517  }
518  return nbsol;
519 
520 }
521 
576 unsigned int
577 vpViper::getInverseKinematics(const vpHomogeneousMatrix & fMc, vpColVector & q, const bool &verbose) const
578 {
581  vpHomogeneousMatrix eMc_;
582  this->get_wMe(wMe);
583  this->get_eMc(eMc_);
584  fMw = fMc * eMc_.inverse() * wMe.inverse();
585 
586  return (getInverseKinematicsWrist(fMw, q, verbose));
587 }
588 
615 vpViper::get_fMc (const vpColVector & q) const
616 {
618  get_fMc(q, fMc);
619 
620  return fMc;
621 }
622 
644 void
646 {
647 
648  // Compute the direct geometric model: fMe = transformation between
649  // fix and end effector frame.
651 
652  get_fMe(q, fMe);
653 
654  fMc = fMe * this->eMc;
655 
656  return;
657 }
658 
731 void
733 {
734  double q1 = q[0];
735  double q2 = q[1];
736  double q3 = q[2];
737  double q4 = q[3];
738  double q5 = q[4];
739  double q6 = q[5];
740  // We turn off the coupling since the measured positions are joint position
741  // taking into account the coupling factor. The coupling factor is relevant
742  // if positions are motor position.
743  // double q6 = q[5] + c56 * q[4];
744 
745 // std::cout << "q6 motor: " << q[5] << " rad "
746 // << vpMath::deg(q[5]) << " deg" << std::endl;
747 // std::cout << "q6 joint: " << q6 << " rad "
748 // << vpMath::deg(q6) << " deg" << std::endl;
749 
750  double c1 = cos(q1);
751  double s1 = sin(q1);
752  double c2 = cos(q2);
753  double s2 = sin(q2);
754  //double c3 = cos(q3);
755  //double s3 = sin(q3);
756  double c4 = cos(q4);
757  double s4 = sin(q4);
758  double c5 = cos(q5);
759  double s5 = sin(q5);
760  double c6 = cos(q6);
761  double s6 = sin(q6);
762  double c23 = cos(q2+q3);
763  double s23 = sin(q2+q3);
764 
765  fMe[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
766  fMe[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
767  fMe[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
768 
769  fMe[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
770  fMe[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
771  fMe[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
772 
773  fMe[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
774  fMe[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
775  fMe[2][2] = -s23*c4*s5+c23*c5;
776 
777  fMe[0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
778  fMe[1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
779  fMe[2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
780 
781  // std::cout << "Effector position fMe: " << std::endl << fMe;
782 
783  return;
784 }
827 void
829 {
830  double q1 = q[0];
831  double q2 = q[1];
832  double q3 = q[2];
833  double q4 = q[3];
834  double q5 = q[4];
835  double q6 = q[5];
836  // We turn off the coupling since the measured positions are joint position
837  // taking into account the coupling factor. The coupling factor is relevant
838  // if positions are motor position.
839  // double q6 = q[5] + c56 * q[4];
840 
841 // std::cout << "q6 motor: " << q[5] << " rad "
842 // << vpMath::deg(q[5]) << " deg" << std::endl;
843 // std::cout << "q6 joint: " << q6 << " rad "
844 // << vpMath::deg(q6) << " deg" << std::endl;
845 
846  double c1 = cos(q1);
847  double s1 = sin(q1);
848  double c2 = cos(q2);
849  double s2 = sin(q2);
850  // double c3 = cos(q3);
851  //double s3 = sin(q3);
852  double c4 = cos(q4);
853  double s4 = sin(q4);
854  double c5 = cos(q5);
855  double s5 = sin(q5);
856  double c6 = cos(q6);
857  double s6 = sin(q6);
858  double c23 = cos(q2+q3);
859  double s23 = sin(q2+q3);
860 
861  fMw[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
862  fMw[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
863  fMw[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
864 
865  fMw[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
866  fMw[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
867  fMw[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
868 
869  fMw[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
870  fMw[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
871  fMw[2][2] = -s23*c4*s5+c23*c5;
872 
873  fMw[0][3] = c1*(-c23*a3+s23*d4+a1+a2*c2);
874  fMw[1][3] = s1*(-c23*a3+s23*d4+a1+a2*c2);
875  fMw[2][3] = s23*a3+c23*d4-a2*s2+d1;
876 
877  //std::cout << "Wrist position fMw: " << std::endl << fMw;
878 
879  return;
880 }
881 
892 void
894 {
895  // Set the rotation as identity
896  wMe.setIdentity();
897 
898  // Set the translation
899  wMe[2][3] = d6;
900 }
901 
913 void
915 {
916  eMc_ = this->eMc;
917 }
918 
930 void
932 {
933  cMe = this->eMc.inverse();
934 }
935 
951 void
953 {
954  vpHomogeneousMatrix cMe ;
955  get_cMe(cMe) ;
956 
957  cVe.buildFrom(cMe) ;
958 
959  return;
960 }
961 
984 void
985 vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
986 {
987  vpMatrix V(6,6);
988  V = 0;
989  // Compute the first and last block of V
991  get_fMw(q, fMw);
992  vpRotationMatrix fRw;
993  fMw.extract(fRw);
994  vpRotationMatrix wRf;
995  wRf = fRw.inverse();
996  for (unsigned int i=0; i<3; i++ ) {
997  for (unsigned int j=0; j<3; j++ ) {
998  V[i][j] = V[i+3][j+3] = wRf[i][j];
999  }
1000  }
1001  // Compute the second block of V
1002  vpHomogeneousMatrix wMe;
1003  get_wMe(wMe);
1004  vpHomogeneousMatrix eMw;
1005  eMw = wMe.inverse();
1006  vpTranslationVector etw;
1007  eMw.extract(etw);
1008  vpMatrix block2 = etw.skew()*wRf;
1009  for (unsigned int i=0; i<3; i++ ) {
1010  for (unsigned int j=0; j<3; j++ ) {
1011  V[i][j+3] = block2[i][j];
1012  }
1013  }
1014  // Compute eJe
1015  vpMatrix fJw;
1016  get_fJw(q, fJw);
1017  eJe = V * fJw;
1018 
1019  return;
1020 }
1021 
1022 
1070 void
1072 {
1073  double q1 = q[0];
1074  double q2 = q[1];
1075  double q3 = q[2];
1076  double q4 = q[3];
1077  double q5 = q[4];
1078 
1079  double c1 = cos(q1);
1080  double s1 = sin(q1);
1081  double c2 = cos(q2);
1082  double s2 = sin(q2);
1083  double c3 = cos(q3);
1084  double s3 = sin(q3);
1085  double c4 = cos(q4);
1086  double s4 = sin(q4);
1087  double c5 = cos(q5);
1088  double s5 = sin(q5);
1089  double c23 = cos(q2+q3);
1090  double s23 = sin(q2+q3);
1091 
1092  vpColVector J1(6);
1093  vpColVector J2(6);
1094  vpColVector J3(6);
1095  vpColVector J4(6);
1096  vpColVector J5(6);
1097  vpColVector J6(6);
1098 
1099  // Jacobian when d6 is set to zero
1100  J1[0] = -s1*(-c23*a3+s23*d4+a1+a2*c2);
1101  J1[1] = c1*(-c23*a3+s23*d4+a1+a2*c2);
1102  J1[2] = 0;
1103  J1[3] = 0;
1104  J1[4] = 0;
1105  J1[5] = 1;
1106 
1107  J2[0] = c1*(s23*a3+c23*d4-a2*s2);
1108  J2[1] = s1*(s23*a3+c23*d4-a2*s2);
1109  J2[2] = c23*a3-s23*d4-a2*c2;
1110  J2[3] = -s1;
1111  J2[4] = c1;
1112  J2[5] = 0;
1113 
1114  J3[0] = c1*(a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*d4);
1115  J3[1] = s1*(a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*d4);
1116  J3[2] = -a3*(s2*s3-c2*c3)-d4*(s2*c3+c2*s3);
1117  J3[3] = -s1;
1118  J3[4] = c1;
1119  J3[5] = 0;
1120 
1121  J4[0] = 0;
1122  J4[1] = 0;
1123  J4[2] = 0;
1124  J4[3] = c1*s23;
1125  J4[4] = s1*s23;
1126  J4[5] = c23;
1127 
1128  J5[0] = 0;
1129  J5[1] = 0;
1130  J5[2] = 0;
1131  J5[3] = -c23*c1*s4-s1*c4;
1132  J5[4] = c1*c4-c23*s1*s4;
1133  J5[5] = s23*s4;
1134 
1135  J6[0] = 0;
1136  J6[1] = 0;
1137  J6[2] = 0;
1138  J6[3] = (c1*c23*c4-s1*s4)*s5+c1*s23*c5;
1139  J6[4] = (s1*c23*c4+c1*s4)*s5+s1*s23*c5;
1140  J6[5] = -s23*c4*s5+c23*c5;
1141 
1142  fJw.resize(6,6) ;
1143  for (unsigned int i=0;i<6;i++) {
1144  fJw[i][0] = J1[i];
1145  fJw[i][1] = J2[i];
1146  fJw[i][2] = J3[i];
1147  fJw[i][3] = J4[i];
1148  fJw[i][4] = J5[i];
1149  fJw[i][5] = J6[i];
1150  }
1151  return;
1152 }
1176 void
1178 {
1179  vpMatrix V(6,6);
1180  V = 0;
1181  // Set the first and last block to identity
1182  for (unsigned int i=0; i<6; i++ )
1183  V[i][i] = 1;
1184 
1185  // Compute the second block of V
1186  vpHomogeneousMatrix fMw;
1187  get_fMw(q, fMw);
1188  vpRotationMatrix fRw;
1189  fMw.extract(fRw);
1190  vpHomogeneousMatrix wMe;
1191  get_wMe(wMe);
1192  vpHomogeneousMatrix eMw;
1193  eMw = wMe.inverse();
1194  vpTranslationVector etw;
1195  eMw.extract(etw);
1196  vpMatrix block2 = (fRw*etw).skew();
1197  // Set the second block
1198  for (unsigned int i=0; i<3; i++ )
1199  for (unsigned int j=0; j<3; j++ )
1200  V[i][j+3] = block2[i][j];
1201 
1202  // Compute fJe
1203  vpMatrix fJw;
1204  get_fJw(q, fJw);
1205  fJe = V * fJw;
1206 
1207  return;
1208 }
1209 
1210 
1220 {
1221  return joint_min;
1222 }
1223 
1233 {
1234  return joint_max;
1235 }
1236 
1247 double
1249 {
1250  return c56;
1251 }
1252 
1262 VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpViper & viper)
1263 {
1264  vpRotationMatrix eRc;
1265  viper.eMc.extract(eRc);
1266  vpRxyzVector rxyz(eRc);
1267 
1268  // Convert joint limits in degrees
1269  vpColVector jmax = viper.joint_max;
1270  vpColVector jmin = viper.joint_min;
1271  jmax.rad2deg();
1272  jmin.rad2deg();
1273 
1274  os
1275  << "Joint Max (deg):" << std::endl
1276  << "\t" << jmax.t() << std::endl
1277 
1278  << "Joint Min (deg): " << std::endl
1279  << "\t" << jmin.t() << std::endl
1280 
1281  << "Coupling 5-6:" << std::endl
1282  << "\t" << viper.c56 << std::endl
1283 
1284  << "eMc: "<< std::endl
1285  << "\tTranslation (m): "
1286  << viper.eMc[0][3] << " "
1287  << viper.eMc[1][3] << " "
1288  << viper.eMc[2][3]
1289  << "\t" << std::endl
1290  << "\tRotation Rxyz (rad) : "
1291  << rxyz[0] << " "
1292  << rxyz[1] << " "
1293  << rxyz[2]
1294  << "\t" << std::endl
1295  << "\tRotation Rxyz (deg) : "
1296  << vpMath::deg(rxyz[0]) << " "
1297  << vpMath::deg(rxyz[1]) << " "
1298  << vpMath::deg(rxyz[2])
1299  << "\t" << std::endl;
1300 
1301  return os;
1302 }
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpViper.cpp:952
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:985
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
Definition: vpViper.cpp:828
double a3
for joint 3
Definition: vpViper.h:154
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:146
vpRotationMatrix inverse() const
invert the rotation matrix
Modelisation of the ADEPT Viper robot.
Definition: vpViper.h:111
double getCoupl56() const
Definition: vpViper.cpp:1248
unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:232
void get_wMe(vpHomogeneousMatrix &wMe) const
Definition: vpViper.cpp:893
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setIdentity()
Basic initialisation (identity)
void get_eMc(vpHomogeneousMatrix &eMc) const
Definition: vpViper.cpp:914
double a2
for joint 2
Definition: vpViper.h:153
The vpRotationMatrix considers the particular case of a rotation matrix.
double d1
for joint 1
Definition: vpViper.h:152
double c56
Mechanical coupling between joint 5 and joint 6.
Definition: vpViper.h:157
double a1
Definition: vpViper.h:152
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
Definition: vpMath.h:106
vpRowVector t() const
Transpose of a vector.
vpColVector joint_max
Definition: vpViper.h:160
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:615
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:732
void get_fJw(const vpColVector &q, vpMatrix &fJw) const
Definition: vpViper.cpp:1071
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:577
static double rad(double deg)
Definition: vpMath.h:100
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1177
vpColVector getJointMin() const
Definition: vpViper.cpp:1219
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:931
static double deg(double rad)
Definition: vpMath.h:93
vpColVector getJointMax() const
Definition: vpViper.cpp:1232
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpHomogeneousMatrix inverse() const
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
double d4
for joint 4
Definition: vpViper.h:155
vpViper()
Definition: vpViper.cpp:70
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpViper.cpp:129
Class that consider the case of a translation vector.
void rad2deg()
Definition: vpColVector.h:182
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:143
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98
double d6
for joint 6
Definition: vpViper.h:156
vpColVector joint_min
Definition: vpViper.h:161