Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
vpViper.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for a generic ADEPT Viper (either 650 or 850) robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
47 #include <cmath> // std::fabs
48 #include <limits> // numeric_limits
49 #include <visp3/core/vpCameraParameters.h>
50 #include <visp3/core/vpDebug.h>
51 #include <visp3/core/vpRotationMatrix.h>
52 #include <visp3/core/vpRxyzVector.h>
53 #include <visp3/core/vpTranslationVector.h>
54 #include <visp3/core/vpVelocityTwistMatrix.h>
55 #include <visp3/robot/vpRobotException.h>
56 #include <visp3/robot/vpViper.h>
57 
58 const unsigned int vpViper::njoint = 6;
59 
66  : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), d7(0), c56(0), joint_max(), joint_min()
67 {
68  // Default values are initialized
69 
70  // Denavit Hartenberg parameters
71  a1 = 0.075;
72  a2 = 0.365;
73  a3 = 0.090;
74  d1 = 0.335;
75  d4 = 0.405;
76  d6 = 0.080;
77  d7 = 0.0666;
78  c56 = -341.33 / 9102.22;
79 
80  // Software joint limits in radians
82  joint_min[0] = vpMath::rad(-170);
83  joint_min[1] = vpMath::rad(-190);
84  joint_min[2] = vpMath::rad(-29);
85  joint_min[3] = vpMath::rad(-190);
86  joint_min[4] = vpMath::rad(-120);
87  joint_min[5] = vpMath::rad(-360);
89  joint_max[0] = vpMath::rad(170);
90  joint_max[1] = vpMath::rad(45);
91  joint_max[2] = vpMath::rad(256);
92  joint_max[3] = vpMath::rad(190);
93  joint_max[4] = vpMath::rad(120);
94  joint_max[5] = vpMath::rad(360);
95 
96  // End effector to camera transformation
97  eMc.eye();
98 }
99 
122 {
124  fMc = get_fMc(q);
125 
126  return fMc;
127 }
128 
143 bool vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod,
144  const bool &verbose) const
145 {
146  double eps = 0.01;
147  if (q >= joint_min[joint] - eps && q <= joint_max[joint] + eps) {
148  q_mod = q;
149  return true;
150  }
151 
152  q_mod = q + 2 * M_PI;
153  if (q_mod >= joint_min[joint] - eps && q_mod <= joint_max[joint] + eps) {
154  return true;
155  }
156 
157  q_mod = q - 2 * M_PI;
158  if (q_mod >= joint_min[joint] - eps && q_mod <= joint_max[joint] + eps) {
159  return true;
160  }
161 
162  if (verbose) {
163  std::cout << "Joint " << joint << " not in limits: " << this->joint_min[joint] << " < " << q << " < "
164  << this->joint_max[joint] << std::endl;
165  }
166 
167  return false;
168 }
169 
223  const bool &verbose) const
224 {
225  vpColVector q_sol[8];
226 
227  for (unsigned int i = 0; i < 8; i++)
228  q_sol[i].resize(6);
229 
230  double c1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
231  double s1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
232  double c3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
233  double s3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
234  double c23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
235  double s23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
236  double c4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
237  double s4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
238  double c5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
239  double s5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
240  double c6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
241  double s6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
242 
243  bool ok[8];
244 
245  if (q.getRows() != njoint)
246  q.resize(6);
247 
248  for (unsigned int i = 0; i < 8; i++)
249  ok[i] = true;
250 
251  double px = fMw[0][3]; // a*c1
252  double py = fMw[1][3]; // a*s1
253  double pz = fMw[2][3];
254 
255  // Compute q1
256  double a_2 = px * px + py * py;
257  // if (a_2 == 0) {// singularity
258  if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) { // singularity
259  c1[0] = cos(q[0]);
260  s1[0] = sin(q[0]);
261  c1[4] = cos(q[0] + M_PI);
262  s1[4] = sin(q[0] + M_PI);
263  } else {
264  double a = sqrt(a_2);
265  c1[0] = px / a;
266  s1[0] = py / a;
267  c1[4] = -px / a;
268  s1[4] = -py / a;
269  }
270 
271  double q1_mod;
272  for (unsigned int i = 0; i < 8; i += 4) {
273  q_sol[i][0] = atan2(s1[i], c1[i]);
274  if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) == true) {
275  q_sol[i][0] = q1_mod;
276  for (unsigned int j = 1; j < 4; j++) {
277  c1[i + j] = c1[i];
278  s1[i + j] = s1[i];
279  q_sol[i + j][0] = q_sol[i][0];
280  }
281  } else {
282  for (unsigned int j = 1; j < 4; j++)
283  ok[i + j] = false;
284  }
285  }
286 
287  // Compute q3
288  double K, q3_mod;
289  for (unsigned int i = 0; i < 8; i += 4) {
290  if (ok[i] == true) {
291  K = (px * px + py * py + pz * pz + a1 * a1 - a2 * a2 - a3 * a3 + d1 * d1 - d4 * d4 -
292  2 * (a1 * c1[i] * px + a1 * s1[i] * py + d1 * pz)) /
293  (2 * a2);
294  double d4_a3_K = d4 * d4 + a3 * a3 - K * K;
295 
296  q_sol[i][2] = atan2(a3, d4) + atan2(K, sqrt(d4_a3_K));
297  q_sol[i + 2][2] = atan2(a3, d4) + atan2(K, -sqrt(d4_a3_K));
298 
299  for (unsigned int j = 0; j < 4; j += 2) {
300  if (d4_a3_K < 0) {
301  for (unsigned int k = 0; k < 2; k++)
302  ok[i + j + k] = false;
303  } else {
304  if (convertJointPositionInLimits(2, q_sol[i + j][2], q3_mod, verbose) == true) {
305  for (unsigned int k = 0; k < 2; k++) {
306  q_sol[i + j + k][2] = q3_mod;
307  c3[i + j + k] = cos(q3_mod);
308  s3[i + j + k] = sin(q3_mod);
309  }
310  } else {
311  for (unsigned int k = 0; k < 2; k++)
312  ok[i + j + k] = false;
313  }
314  }
315  }
316  }
317  }
318  // std::cout << "ok apres q3: ";
319  // for (unsigned int i=0; i< 8; i++)
320  // std::cout << ok[i] << " ";
321  // std::cout << std::endl;
322 
323  // Compute q2
324  double q23[8], q2_mod;
325  for (unsigned int i = 0; i < 8; i += 2) {
326  if (ok[i] == true) {
327  // Compute q23 = q2+q3
328  c23[i] = (-(a3 - a2 * c3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (d4 + a2 * s3[i])) /
329  ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
330  s23[i] = ((d4 + a2 * s3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (a3 - a2 * c3[i])) /
331  ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
332  q23[i] = atan2(s23[i], c23[i]);
333  // std::cout << i << " c23 = " << c23[i] << " s23 = " << s23[i] <<
334  // std::endl;
335  // q2 = q23 - q3
336  q_sol[i][1] = q23[i] - q_sol[i][2];
337 
338  if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) == true) {
339  for (unsigned int j = 0; j < 2; j++) {
340  q_sol[i + j][1] = q2_mod;
341  c23[i + j] = c23[i];
342  s23[i + j] = s23[i];
343  }
344  } else {
345  for (unsigned int j = 0; j < 2; j++)
346  ok[i + j] = false;
347  }
348  }
349  }
350  // std::cout << "ok apres q2: ";
351  // for (unsigned int i=0; i< 8; i++)
352  // std::cout << ok[i] << " ";
353  // std::cout << std::endl;
354 
355  // Compute q4 as long as s5 != 0
356  double r13 = fMw[0][2];
357  double r23 = fMw[1][2];
358  double r33 = fMw[2][2];
359  double s4s5, c4s5, q4_mod, q5_mod;
360  for (unsigned int i = 0; i < 8; i += 2) {
361  if (ok[i] == true) {
362  s4s5 = -s1[i] * r13 + c1[i] * r23;
363  c4s5 = c1[i] * c23[i] * r13 + s1[i] * c23[i] * r23 - s23[i] * r33;
364  if (fabs(s4s5) < vpMath::rad(0.5) && fabs(c4s5) < vpMath::rad(0.5)) {
365  // s5 = 0
366  c5[i] = c1[i] * s23[i] * r13 + s1[i] * s23[i] * r23 + c23[i] * r33;
367  // std::cout << "Singularity: s5 near 0: ";
368  if (c5[i] > 0.)
369  q_sol[i][4] = 0.0;
370  else
371  q_sol[i][4] = M_PI;
372 
373  if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) == true) {
374  for (unsigned int j = 0; j < 2; j++) {
375  q_sol[i + j][3] = q[3]; // keep current q4
376  q_sol[i + j][4] = q5_mod;
377  c4[i] = cos(q_sol[i + j][3]);
378  s4[i] = sin(q_sol[i + j][3]);
379  }
380  } else {
381  for (unsigned int j = 0; j < 2; j++)
382  ok[i + j] = false;
383  }
384  } else {
385 // s5 != 0
386 #if 0 // Modified 2016/03/10 since if and else are the same
387  // if (c4s5 == 0) {
388  if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
389  // c4 = 0
390  // vpTRACE("c4 = 0");
391  // q_sol[i][3] = q[3]; // keep current position
392  q_sol[i][3] = atan2(s4s5, c4s5);
393  }
394  else {
395  q_sol[i][3] = atan2(s4s5, c4s5);
396  }
397 #else
398  q_sol[i][3] = atan2(s4s5, c4s5);
399 #endif
400  if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) == true) {
401  q_sol[i][3] = q4_mod;
402  c4[i] = cos(q4_mod);
403  s4[i] = sin(q4_mod);
404  } else {
405  ok[i] = false;
406  }
407  if (q_sol[i][3] > 0.)
408  q_sol[i + 1][3] = q_sol[i][3] + M_PI;
409  else
410  q_sol[i + 1][3] = q_sol[i][3] - M_PI;
411  if (convertJointPositionInLimits(3, q_sol[i + 1][3], q4_mod, verbose) == true) {
412  q_sol[i + 1][3] = q4_mod;
413  c4[i + 1] = cos(q4_mod);
414  s4[i + 1] = sin(q4_mod);
415  } else {
416  ok[i + 1] = false;
417  }
418 
419  // Compute q5
420  for (unsigned int j = 0; j < 2; j++) {
421  if (ok[i + j] == true) {
422  c5[i + j] = c1[i + j] * s23[i + j] * r13 + s1[i + j] * s23[i + j] * r23 + c23[i + j] * r33;
423  s5[i + j] = (c1[i + j] * c23[i + j] * c4[i + j] - s1[i + j] * s4[i + j]) * r13 +
424  (s1[i + j] * c23[i + j] * c4[i + j] + c1[i + j] * s4[i + j]) * r23 -
425  s23[i + j] * c4[i + j] * r33;
426 
427  q_sol[i + j][4] = atan2(s5[i + j], c5[i + j]);
428  if (convertJointPositionInLimits(4, q_sol[i + j][4], q5_mod, verbose) == true) {
429  q_sol[i + j][4] = q5_mod;
430  } else {
431 
432  ok[i + j] = false;
433  }
434  }
435  }
436  }
437  }
438  }
439 
440  // Compute q6
441  // 4 solutions for q6 and 4 more solutions by flipping the wrist (see below)
442  double r12 = fMw[0][1];
443  double r22 = fMw[1][1];
444  double r32 = fMw[2][1];
445  double q6_mod;
446  for (unsigned int i = 0; i < 8; i++) {
447  c6[i] = -(c1[i] * c23[i] * s4[i] + s1[i] * c4[i]) * r12 + (c1[i] * c4[i] - s1[i] * c23[i] * s4[i]) * r22 +
448  s23[i] * s4[i] * r32;
449  s6[i] = -(c1[i] * c23[i] * c4[i] * c5[i] - c1[i] * s23[i] * s5[i] - s1[i] * s4[i] * c5[i]) * r12 -
450  (s1[i] * c23[i] * c4[i] * c5[i] - s1[i] * s23[i] * s5[i] + c1[i] * s4[i] * c5[i]) * r22 +
451  (c23[i] * s5[i] + s23[i] * c4[i] * c5[i]) * r32;
452 
453  q_sol[i][5] = atan2(s6[i], c6[i]);
454  if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) == true) {
455  q_sol[i][5] = q6_mod;
456  } else {
457  ok[i] = false;
458  }
459  }
460 
461  // Select the best config in terms of distance from the current position
462  unsigned int nbsol = 0;
463  unsigned int sol = 0;
464  vpColVector dist(8);
465  for (unsigned int i = 0; i < 8; i++) {
466  if (ok[i] == true) {
467  nbsol++;
468  sol = i;
469  // dist[i] = vpColVector::distance(q, q_sol[i]);
470  vpColVector weight(6);
471  weight = 1;
472  weight[0] = 8;
473  weight[1] = weight[2] = 4;
474  dist[i] = 0;
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;
481  } else {
482  if (fabs(rought_dist + 2 * M_PI) < fabs(rought_dist))
483  modulo_dist = rought_dist + 2 * M_PI;
484  }
485  // std::cout << "dist " << i << ": " << rought_dist << " modulo: " <<
486  // modulo_dist << std::endl;
487  dist[i] += weight[j] * vpMath::sqr(modulo_dist);
488  }
489  }
490  // std::cout << "sol " << i << " [" << ok[i] << "] dist: " << dist[i] <<
491  // " q: " << q_sol[i].t() << std::endl;
492  }
493  // std::cout << "dist: " << dist.t() << std::endl;
494  if (nbsol) {
495  for (unsigned int i = 0; i < 8; i++) {
496  if (ok[i] == true)
497  if (dist[i] < dist[sol])
498  sol = i;
499  }
500  // Update the inverse kinematics solution
501  q = q_sol[sol];
502 
503  // std::cout << "Nearest solution (" << sol << ") with distance ("
504  // << dist[sol] << "): " << q_sol[sol].t() << std::endl;
505  }
506  return nbsol;
507 }
508 
563 unsigned int vpViper::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose) const
564 {
567  vpHomogeneousMatrix eMc_;
568  this->get_wMe(wMe);
569  this->get_eMc(eMc_);
570  fMw = fMc * eMc_.inverse() * wMe.inverse();
571 
572  return (getInverseKinematicsWrist(fMw, q, verbose));
573 }
574 
601 {
603  get_fMc(q, fMc);
604 
605  return fMc;
606 }
607 
630 {
631 
632  // Compute the direct geometric model: fMe = transformation between
633  // fix and end effector frame.
635 
636  get_fMe(q, fMe);
637 
638  fMc = fMe * this->eMc;
639 
640  return;
641 }
642 
716 {
717  double q1 = q[0];
718  double q2 = q[1];
719  double q3 = q[2];
720  double q4 = q[3];
721  double q5 = q[4];
722  double q6 = q[5];
723  // We turn off the coupling since the measured positions are joint position
724  // taking into account the coupling factor. The coupling factor is relevant
725  // if positions are motor position.
726  // double q6 = q[5] + c56 * q[4];
727 
728  // std::cout << "q6 motor: " << q[5] << " rad "
729  // << vpMath::deg(q[5]) << " deg" << std::endl;
730  // std::cout << "q6 joint: " << q6 << " rad "
731  // << vpMath::deg(q6) << " deg" << std::endl;
732 
733  double c1 = cos(q1);
734  double s1 = sin(q1);
735  double c2 = cos(q2);
736  double s2 = sin(q2);
737  // double c3 = cos(q3);
738  // double s3 = sin(q3);
739  double c4 = cos(q4);
740  double s4 = sin(q4);
741  double c5 = cos(q5);
742  double s5 = sin(q5);
743  double c6 = cos(q6);
744  double s6 = sin(q6);
745  double c23 = cos(q2 + q3);
746  double s23 = sin(q2 + q3);
747 
748  fMe[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
749  fMe[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
750  fMe[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
751 
752  fMe[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
753  fMe[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
754  fMe[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
755 
756  fMe[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
757  fMe[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
758  fMe[2][2] = -s23 * c4 * s5 + c23 * c5;
759 
760  fMe[0][3] = c1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) - s1 * s4 * s5 * d6;
761  fMe[1][3] = s1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) + c1 * s4 * s5 * d6;
762  fMe[2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;
763 
764  // std::cout << "Effector position fMe: " << std::endl << fMe;
765 
766  return;
767 }
811 {
812  double q1 = q[0];
813  double q2 = q[1];
814  double q3 = q[2];
815  double q4 = q[3];
816  double q5 = q[4];
817  double q6 = q[5];
818  // We turn off the coupling since the measured positions are joint position
819  // taking into account the coupling factor. The coupling factor is relevant
820  // if positions are motor position.
821  // double q6 = q[5] + c56 * q[4];
822 
823  // std::cout << "q6 motor: " << q[5] << " rad "
824  // << vpMath::deg(q[5]) << " deg" << std::endl;
825  // std::cout << "q6 joint: " << q6 << " rad "
826  // << vpMath::deg(q6) << " deg" << std::endl;
827 
828  double c1 = cos(q1);
829  double s1 = sin(q1);
830  double c2 = cos(q2);
831  double s2 = sin(q2);
832  // double c3 = cos(q3);
833  // double s3 = sin(q3);
834  double c4 = cos(q4);
835  double s4 = sin(q4);
836  double c5 = cos(q5);
837  double s5 = sin(q5);
838  double c6 = cos(q6);
839  double s6 = sin(q6);
840  double c23 = cos(q2 + q3);
841  double s23 = sin(q2 + q3);
842 
843  fMw[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
844  fMw[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
845  fMw[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
846 
847  fMw[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
848  fMw[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
849  fMw[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
850 
851  fMw[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
852  fMw[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
853  fMw[2][2] = -s23 * c4 * s5 + c23 * c5;
854 
855  fMw[0][3] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
856  fMw[1][3] = s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
857  fMw[2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
858 
859  // std::cout << "Wrist position fMw: " << std::endl << fMw;
860 
861  return;
862 }
863 
875 {
876  // Set the rotation as identity
877  wMe.eye();
878 
879  // Set the translation
880  wMe[2][3] = d6;
881 }
882 
894 void vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const { eMc_ = this->eMc; }
895 
906 {
907  eMs.eye();
908  eMs[2][3] = -d7; // tz = -d7
909 }
910 
922 void vpViper::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->eMc.inverse(); }
923 
939 {
941  get_cMe(cMe);
942 
943  cVe.buildFrom(cMe);
944 
945  return;
946 }
947 
970 void vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
971 {
972  vpMatrix V(6, 6);
973  V = 0;
974  // Compute the first and last block of V
976  get_fMw(q, fMw);
977  vpRotationMatrix fRw;
978  fMw.extract(fRw);
979  vpRotationMatrix wRf;
980  wRf = fRw.inverse();
981  for (unsigned int i = 0; i < 3; i++) {
982  for (unsigned int j = 0; j < 3; j++) {
983  V[i][j] = V[i + 3][j + 3] = wRf[i][j];
984  }
985  }
986  // Compute the second block of V
988  get_wMe(wMe);
990  eMw = wMe.inverse();
992  eMw.extract(etw);
993  vpMatrix block2 = etw.skew() * wRf;
994  for (unsigned int i = 0; i < 3; i++) {
995  for (unsigned int j = 0; j < 3; j++) {
996  V[i][j + 3] = block2[i][j];
997  }
998  }
999  // Compute eJe
1000  vpMatrix fJw;
1001  get_fJw(q, fJw);
1002  eJe = V * fJw;
1003 
1004  return;
1005 }
1006 
1054 void vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
1055 {
1056  double q1 = q[0];
1057  double q2 = q[1];
1058  double q3 = q[2];
1059  double q4 = q[3];
1060  double q5 = q[4];
1061 
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);
1074 
1075  vpColVector J1(6);
1076  vpColVector J2(6);
1077  vpColVector J3(6);
1078  vpColVector J4(6);
1079  vpColVector J5(6);
1080  vpColVector J6(6);
1081 
1082  // Jacobian when d6 is set to zero
1083  J1[0] = -s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
1084  J1[1] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
1085  J1[2] = 0;
1086  J1[3] = 0;
1087  J1[4] = 0;
1088  J1[5] = 1;
1089 
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;
1093  J2[3] = -s1;
1094  J2[4] = c1;
1095  J2[5] = 0;
1096 
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);
1100  J3[3] = -s1;
1101  J3[4] = c1;
1102  J3[5] = 0;
1103 
1104  J4[0] = 0;
1105  J4[1] = 0;
1106  J4[2] = 0;
1107  J4[3] = c1 * s23;
1108  J4[4] = s1 * s23;
1109  J4[5] = c23;
1110 
1111  J5[0] = 0;
1112  J5[1] = 0;
1113  J5[2] = 0;
1114  J5[3] = -c23 * c1 * s4 - s1 * c4;
1115  J5[4] = c1 * c4 - c23 * s1 * s4;
1116  J5[5] = s23 * s4;
1117 
1118  J6[0] = 0;
1119  J6[1] = 0;
1120  J6[2] = 0;
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;
1124 
1125  fJw.resize(6, 6);
1126  for (unsigned int i = 0; i < 6; i++) {
1127  fJw[i][0] = J1[i];
1128  fJw[i][1] = J2[i];
1129  fJw[i][2] = J3[i];
1130  fJw[i][3] = J4[i];
1131  fJw[i][4] = J5[i];
1132  fJw[i][5] = J6[i];
1133  }
1134  return;
1135 }
1159 void vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1160 {
1161  vpMatrix V(6, 6);
1162  V = 0;
1163  // Set the first and last block to identity
1164  for (unsigned int i = 0; i < 6; i++)
1165  V[i][i] = 1;
1166 
1167  // Compute the second block of V
1168  vpHomogeneousMatrix fMw;
1169  get_fMw(q, fMw);
1170  vpRotationMatrix fRw;
1171  fMw.extract(fRw);
1172  vpHomogeneousMatrix wMe;
1173  get_wMe(wMe);
1174  vpHomogeneousMatrix eMw;
1175  eMw = wMe.inverse();
1176  vpTranslationVector etw;
1177  eMw.extract(etw);
1178  vpMatrix block2 = (fRw * etw).skew();
1179  // Set the second block
1180  for (unsigned int i = 0; i < 3; i++)
1181  for (unsigned int j = 0; j < 3; j++)
1182  V[i][j + 3] = block2[i][j];
1183 
1184  // Compute fJe
1185  vpMatrix fJw;
1186  get_fJw(q, fJw);
1187  fJe = V * fJw;
1188 
1189  return;
1190 }
1191 
1200 
1209 
1220 double vpViper::getCoupl56() const { return c56; }
1221 
1231 {
1232  this->eMc = eMc_;
1233  this->eMc.extract(etc);
1234  vpRotationMatrix R(this->eMc);
1235  this->erc.buildFrom(R);
1236 }
1237 
1248 void vpViper::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_)
1249 {
1250  this->etc = etc_;
1251  this->erc = erc_;
1252  vpRotationMatrix eRc(erc);
1253  this->eMc.buildFrom(etc, eRc);
1254 }
1255 
1265 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpViper &viper)
1266 {
1267  vpRotationMatrix eRc;
1268  viper.eMc.extract(eRc);
1269  vpRxyzVector rxyz(eRc);
1270 
1271  // Convert joint limits in degrees
1272  vpColVector jmax = viper.joint_max;
1273  vpColVector jmin = viper.joint_min;
1274  jmax.rad2deg();
1275  jmin.rad2deg();
1276 
1277  os << "Joint Max (deg):" << std::endl
1278  << "\t" << jmax.t() << std::endl
1279 
1280  << "Joint Min (deg): " << std::endl
1281  << "\t" << jmin.t() << std::endl
1282 
1283  << "Coupling 5-6:" << std::endl
1284  << "\t" << viper.c56 << std::endl
1285 
1286  << "eMc: " << std::endl
1287  << "\tTranslation (m): " << viper.eMc[0][3] << " " << viper.eMc[1][3] << " " << viper.eMc[2][3] << "\t"
1288  << std::endl
1289  << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1290  << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1291  << "\t" << std::endl;
1292 
1293  return os;
1294 }
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpViper.cpp:938
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
double d7
for force/torque location
Definition: vpViper.h:168
vpRxyzVector buildFrom(const vpRotationMatrix &R)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
Definition: vpViper.cpp:810
double a3
for joint 3
Definition: vpViper.h:165
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:157
vpRotationMatrix inverse() const
Modelisation of the ADEPT Viper robot.
Definition: vpViper.h:113
double getCoupl56() const
Definition: vpViper.cpp:1220
unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:222
void get_wMe(vpHomogeneousMatrix &wMe) const
Definition: vpViper.cpp:874
Implementation of an homogeneous matrix and operations on such kind of matrices.
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpViper &viper)
Definition: vpViper.cpp:1265
void get_eMc(vpHomogeneousMatrix &eMc) const
Definition: vpViper.cpp:894
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
void get_eMs(vpHomogeneousMatrix &eMs) const
Definition: vpViper.cpp:905
vpRxyzVector erc
Definition: vpViper.h:160
double a2
for joint 2
Definition: vpViper.h:164
Implementation of a rotation matrix and operations on such kind of matrices.
double d1
for joint 1
Definition: vpViper.h:163
double c56
Mechanical coupling between joint 5 and joint 6.
Definition: vpViper.h:169
double a1
Definition: vpViper.h:163
vpTranslationVector etc
Definition: vpViper.h:159
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1230
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
Definition: vpMath.h:114
vpRowVector t() const
vpColVector joint_max
Definition: vpViper.h:172
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:715
void get_fJw(const vpColVector &q, vpMatrix &fJw) const
Definition: vpViper.cpp:1054
unsigned int getRows() const
Definition: vpArray2D.h:289
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
Definition: vpMath.h:108
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
vpColVector getJointMin() const
Definition: vpViper.cpp:1199
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
static double deg(double rad)
Definition: vpMath.h:101
vpColVector getJointMax() const
Definition: vpViper.cpp:1208
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpHomogeneousMatrix inverse() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
double d4
for joint 4
Definition: vpViper.h:166
vpViper()
Definition: vpViper.cpp:65
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpViper.cpp:121
Class that consider the case of a translation vector.
void rad2deg()
Definition: vpColVector.h:294
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:154
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
double d6
for joint 6
Definition: vpViper.h:167
vpColVector joint_min
Definition: vpViper.h:173