Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
vpViper.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
44 #include <cmath> // std::fabs
45 #include <limits> // numeric_limits
46 #include <visp3/core/vpCameraParameters.h>
47 #include <visp3/core/vpDebug.h>
48 #include <visp3/core/vpRotationMatrix.h>
49 #include <visp3/core/vpRxyzVector.h>
50 #include <visp3/core/vpTranslationVector.h>
51 #include <visp3/core/vpVelocityTwistMatrix.h>
52 #include <visp3/robot/vpRobotException.h>
53 #include <visp3/robot/vpViper.h>
54 
55 const unsigned int vpViper::njoint = 6;
56 
63  : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), d7(0), c56(0), joint_max(), joint_min()
64 {
65  // Default values are initialized
66 
67  // Denavit-Hartenberg parameters
68  a1 = 0.075;
69  a2 = 0.365;
70  a3 = 0.090;
71  d1 = 0.335;
72  d4 = 0.405;
73  d6 = 0.080;
74  d7 = 0.0666;
75  c56 = -341.33 / 9102.22;
76 
77  // Software joint limits in radians
79  joint_min[0] = vpMath::rad(-170);
80  joint_min[1] = vpMath::rad(-190);
81  joint_min[2] = vpMath::rad(-29);
82  joint_min[3] = vpMath::rad(-190);
83  joint_min[4] = vpMath::rad(-120);
84  joint_min[5] = vpMath::rad(-360);
86  joint_max[0] = vpMath::rad(170);
87  joint_max[1] = vpMath::rad(45);
88  joint_max[2] = vpMath::rad(256);
89  joint_max[3] = vpMath::rad(190);
90  joint_max[4] = vpMath::rad(120);
91  joint_max[5] = vpMath::rad(360);
92 
93  // End effector to camera transformation
94  eMc.eye();
95 }
96 
119 {
121  fMc = get_fMc(q);
122 
123  return fMc;
124 }
125 
140 bool vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod,
141  const bool &verbose) const
142 {
143  double eps = 0.01;
144  if (q >= joint_min[joint] - eps && q <= joint_max[joint] + eps) {
145  q_mod = q;
146  return true;
147  }
148 
149  q_mod = q + 2 * M_PI;
150  if (q_mod >= joint_min[joint] - eps && q_mod <= joint_max[joint] + eps) {
151  return true;
152  }
153 
154  q_mod = q - 2 * M_PI;
155  if (q_mod >= joint_min[joint] - eps && q_mod <= joint_max[joint] + eps) {
156  return true;
157  }
158 
159  if (verbose) {
160  std::cout << "Joint " << joint << " not in limits: " << this->joint_min[joint] << " < " << q << " < "
161  << this->joint_max[joint] << std::endl;
162  }
163 
164  return false;
165 }
166 
220  const bool &verbose) const
221 {
222  vpColVector q_sol[8];
223 
224  for (unsigned int i = 0; i < 8; i++)
225  q_sol[i].resize(6);
226 
227  double c1[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
228  double s1[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
229  double c3[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
230  double s3[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
231  double c23[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
232  double s23[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
233  double c4[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
234  double s4[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
235  double c5[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
236  double s5[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
237  double c6[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
238  double s6[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
239 
240  bool ok[8];
241 
242  if (q.getRows() != njoint)
243  q.resize(6);
244 
245  for (unsigned int i = 0; i < 8; i++)
246  ok[i] = true;
247 
248  double px = fMw[0][3]; // a*c1
249  double py = fMw[1][3]; // a*s1
250  double pz = fMw[2][3];
251 
252  // Compute q1
253  double a_2 = px * px + py * py;
254  // if (a_2 == 0) {// singularity
255  if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) { // singularity
256  c1[0] = cos(q[0]);
257  s1[0] = sin(q[0]);
258  c1[4] = cos(q[0] + M_PI);
259  s1[4] = sin(q[0] + M_PI);
260  }
261  else {
262  double a = sqrt(a_2);
263  c1[0] = px / a;
264  s1[0] = py / a;
265  c1[4] = -px / a;
266  s1[4] = -py / a;
267  }
268 
269  double q1_mod;
270  for (unsigned int i = 0; i < 8; i += 4) {
271  q_sol[i][0] = atan2(s1[i], c1[i]);
272  if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) == true) {
273  q_sol[i][0] = q1_mod;
274  for (unsigned int j = 1; j < 4; j++) {
275  c1[i + j] = c1[i];
276  s1[i + j] = s1[i];
277  q_sol[i + j][0] = q_sol[i][0];
278  }
279  }
280  else {
281  for (unsigned int j = 1; j < 4; j++)
282  ok[i + j] = false;
283  }
284  }
285 
286  // Compute q3
287  double K, q3_mod;
288  for (unsigned int i = 0; i < 8; i += 4) {
289  if (ok[i] == true) {
290  K = (px * px + py * py + pz * pz + a1 * a1 - a2 * a2 - a3 * a3 + d1 * d1 - d4 * d4 -
291  2 * (a1 * c1[i] * px + a1 * s1[i] * py + d1 * pz)) /
292  (2 * a2);
293  double d4_a3_K = d4 * d4 + a3 * a3 - K * K;
294 
295  q_sol[i][2] = atan2(a3, d4) + atan2(K, sqrt(d4_a3_K));
296  q_sol[i + 2][2] = atan2(a3, d4) + atan2(K, -sqrt(d4_a3_K));
297 
298  for (unsigned int j = 0; j < 4; j += 2) {
299  if (d4_a3_K < 0) {
300  for (unsigned int k = 0; k < 2; k++)
301  ok[i + j + k] = false;
302  }
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  }
311  else {
312  for (unsigned int k = 0; k < 2; k++)
313  ok[i + j + k] = false;
314  }
315  }
316  }
317  }
318  }
319  // std::cout << "ok apres q3: ";
320  // for (unsigned int i=0; i< 8; i++)
321  // std::cout << ok[i] << " ";
322  // std::cout << std::endl;
323 
324  // Compute q2
325  double q23[8], q2_mod;
326  for (unsigned int i = 0; i < 8; i += 2) {
327  if (ok[i] == true) {
328  // Compute q23 = q2+q3
329  c23[i] = (-(a3 - a2 * c3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (d4 + a2 * s3[i])) /
330  ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
331  s23[i] = ((d4 + a2 * s3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (a3 - a2 * c3[i])) /
332  ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
333  q23[i] = atan2(s23[i], c23[i]);
334  // std::cout << i << " c23 = " << c23[i] << " s23 = " << s23[i] <<
335  // std::endl;
336  // q2 = q23 - q3
337  q_sol[i][1] = q23[i] - q_sol[i][2];
338 
339  if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) == true) {
340  for (unsigned int j = 0; j < 2; j++) {
341  q_sol[i + j][1] = q2_mod;
342  c23[i + j] = c23[i];
343  s23[i + j] = s23[i];
344  }
345  }
346  else {
347  for (unsigned int j = 0; j < 2; j++)
348  ok[i + j] = false;
349  }
350  }
351  }
352  // std::cout << "ok apres q2: ";
353  // for (unsigned int i=0; i< 8; i++)
354  // std::cout << ok[i] << " ";
355  // std::cout << std::endl;
356 
357  // Compute q4 as long as s5 != 0
358  double r13 = fMw[0][2];
359  double r23 = fMw[1][2];
360  double r33 = fMw[2][2];
361  double s4s5, c4s5, q4_mod, q5_mod;
362  for (unsigned int i = 0; i < 8; i += 2) {
363  if (ok[i] == true) {
364  s4s5 = -s1[i] * r13 + c1[i] * r23;
365  c4s5 = c1[i] * c23[i] * r13 + s1[i] * c23[i] * r23 - s23[i] * r33;
366  if (fabs(s4s5) < vpMath::rad(0.5) && fabs(c4s5) < vpMath::rad(0.5)) {
367  // s5 = 0
368  c5[i] = c1[i] * s23[i] * r13 + s1[i] * s23[i] * r23 + c23[i] * r33;
369  // std::cout << "Singularity: s5 near 0: ";
370  if (c5[i] > 0.)
371  q_sol[i][4] = 0.0;
372  else
373  q_sol[i][4] = M_PI;
374 
375  if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) == true) {
376  for (unsigned int j = 0; j < 2; j++) {
377  q_sol[i + j][3] = q[3]; // keep current q4
378  q_sol[i + j][4] = q5_mod;
379  c4[i] = cos(q_sol[i + j][3]);
380  s4[i] = sin(q_sol[i + j][3]);
381  }
382  }
383  else {
384  for (unsigned int j = 0; j < 2; j++)
385  ok[i + j] = false;
386  }
387  }
388  else {
389 // s5 != 0
390 #if 0 // Modified 2016/03/10 since if and else are the same
391  // if (c4s5 == 0) {
392  if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
393  // c4 = 0
394  // vpTRACE("c4 = 0");
395  // q_sol[i][3] = q[3]; // keep current position
396  q_sol[i][3] = atan2(s4s5, c4s5);
397  }
398  else {
399  q_sol[i][3] = atan2(s4s5, c4s5);
400  }
401 #else
402  q_sol[i][3] = atan2(s4s5, c4s5);
403 #endif
404  if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) == true) {
405  q_sol[i][3] = q4_mod;
406  c4[i] = cos(q4_mod);
407  s4[i] = sin(q4_mod);
408  }
409  else {
410  ok[i] = false;
411  }
412  if (q_sol[i][3] > 0.)
413  q_sol[i + 1][3] = q_sol[i][3] + M_PI;
414  else
415  q_sol[i + 1][3] = q_sol[i][3] - M_PI;
416  if (convertJointPositionInLimits(3, q_sol[i + 1][3], q4_mod, verbose) == true) {
417  q_sol[i + 1][3] = q4_mod;
418  c4[i + 1] = cos(q4_mod);
419  s4[i + 1] = sin(q4_mod);
420  }
421  else {
422  ok[i + 1] = false;
423  }
424 
425  // Compute q5
426  for (unsigned int j = 0; j < 2; j++) {
427  if (ok[i + j] == true) {
428  c5[i + j] = c1[i + j] * s23[i + j] * r13 + s1[i + j] * s23[i + j] * r23 + c23[i + j] * r33;
429  s5[i + j] = (c1[i + j] * c23[i + j] * c4[i + j] - s1[i + j] * s4[i + j]) * r13 +
430  (s1[i + j] * c23[i + j] * c4[i + j] + c1[i + j] * s4[i + j]) * r23 -
431  s23[i + j] * c4[i + j] * r33;
432 
433  q_sol[i + j][4] = atan2(s5[i + j], c5[i + j]);
434  if (convertJointPositionInLimits(4, q_sol[i + j][4], q5_mod, verbose) == true) {
435  q_sol[i + j][4] = q5_mod;
436  }
437  else {
438 
439  ok[i + j] = false;
440  }
441  }
442  }
443  }
444  }
445  }
446 
447  // Compute q6
448  // 4 solutions for q6 and 4 more solutions by flipping the wrist (see below)
449  double r12 = fMw[0][1];
450  double r22 = fMw[1][1];
451  double r32 = fMw[2][1];
452  double q6_mod;
453  for (unsigned int i = 0; i < 8; i++) {
454  c6[i] = -(c1[i] * c23[i] * s4[i] + s1[i] * c4[i]) * r12 + (c1[i] * c4[i] - s1[i] * c23[i] * s4[i]) * r22 +
455  s23[i] * s4[i] * r32;
456  s6[i] = -(c1[i] * c23[i] * c4[i] * c5[i] - c1[i] * s23[i] * s5[i] - s1[i] * s4[i] * c5[i]) * r12 -
457  (s1[i] * c23[i] * c4[i] * c5[i] - s1[i] * s23[i] * s5[i] + c1[i] * s4[i] * c5[i]) * r22 +
458  (c23[i] * s5[i] + s23[i] * c4[i] * c5[i]) * r32;
459 
460  q_sol[i][5] = atan2(s6[i], c6[i]);
461  if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) == true) {
462  q_sol[i][5] = q6_mod;
463  }
464  else {
465  ok[i] = false;
466  }
467  }
468 
469  // Select the best config in terms of distance from the current position
470  unsigned int nbsol = 0;
471  unsigned int sol = 0;
472  vpColVector dist(8);
473  for (unsigned int i = 0; i < 8; i++) {
474  if (ok[i] == true) {
475  nbsol++;
476  sol = i;
477  vpColVector weight(6);
478  weight = 1;
479  weight[0] = 8;
480  weight[1] = weight[2] = 4;
481  dist[i] = 0;
482  for (unsigned int j = 0; j < 6; j++) {
483  double rought_dist = q[j] - q_sol[i][j];
484  double modulo_dist = rought_dist;
485  if (rought_dist > 0) {
486  if (fabs(rought_dist - 2 * M_PI) < fabs(rought_dist))
487  modulo_dist = rought_dist - 2 * M_PI;
488  }
489  else {
490  if (fabs(rought_dist + 2 * M_PI) < fabs(rought_dist))
491  modulo_dist = rought_dist + 2 * M_PI;
492  }
493 
494  dist[i] += weight[j] * vpMath::sqr(modulo_dist);
495  }
496  }
497 
498  }
499  // std::cout << "dist: " << dist.t() << std::endl;
500  if (nbsol) {
501  for (unsigned int i = 0; i < 8; i++) {
502  if (ok[i] == true)
503  if (dist[i] < dist[sol])
504  sol = i;
505  }
506  // Update the inverse kinematics solution
507  q = q_sol[sol];
508 
509  }
510  return nbsol;
511 }
512 
567 unsigned int vpViper::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose) const
568 {
571  vpHomogeneousMatrix eMc_;
572  this->get_wMe(wMe);
573  this->get_eMc(eMc_);
574  fMw = fMc * eMc_.inverse() * wMe.inverse();
575 
576  return (getInverseKinematicsWrist(fMw, q, verbose));
577 }
578 
605 {
607  get_fMc(q, fMc);
608 
609  return fMc;
610 }
611 
634 {
635 
636  // Compute the direct geometric model: fMe = transformation between
637  // fix and end effector frame.
639 
640  get_fMe(q, fMe);
641 
642  fMc = fMe * this->eMc;
643 
644  return;
645 }
646 
720 {
721  double q1 = q[0];
722  double q2 = q[1];
723  double q3 = q[2];
724  double q4 = q[3];
725  double q5 = q[4];
726  double q6 = q[5];
727  // We turn off the coupling since the measured positions are joint position
728  // taking into account the coupling factor. The coupling factor is relevant
729  // if positions are motor position.
730  // double q6 = q[5] + c56 * q[4];
731 
732  double c1 = cos(q1);
733  double s1 = sin(q1);
734  double c2 = cos(q2);
735  double s2 = sin(q2);
736  // double c3 = cos(q3);
737  // double s3 = sin(q3);
738  double c4 = cos(q4);
739  double s4 = sin(q4);
740  double c5 = cos(q5);
741  double s5 = sin(q5);
742  double c6 = cos(q6);
743  double s6 = sin(q6);
744  double c23 = cos(q2 + q3);
745  double s23 = sin(q2 + q3);
746 
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;
750 
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;
754 
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;
758 
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;
761  fMe[2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;
762 
763  // std::cout << "Effector position fMe: " << std::endl << fMe;
764 
765  return;
766 }
810 {
811  double q1 = q[0];
812  double q2 = q[1];
813  double q3 = q[2];
814  double q4 = q[3];
815  double q5 = q[4];
816  double q6 = q[5];
817  // We turn off the coupling since the measured positions are joint position
818  // taking into account the coupling factor. The coupling factor is relevant
819  // if positions are motor position.
820  // double q6 = q[5] + c56 * q[4];
821 
822  double c1 = cos(q1);
823  double s1 = sin(q1);
824  double c2 = cos(q2);
825  double s2 = sin(q2);
826  // double c3 = cos(q3);
827  // double s3 = sin(q3);
828  double c4 = cos(q4);
829  double s4 = sin(q4);
830  double c5 = cos(q5);
831  double s5 = sin(q5);
832  double c6 = cos(q6);
833  double s6 = sin(q6);
834  double c23 = cos(q2 + q3);
835  double s23 = sin(q2 + q3);
836 
837  fMw[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
838  fMw[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
839  fMw[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
840 
841  fMw[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
842  fMw[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
843  fMw[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
844 
845  fMw[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
846  fMw[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
847  fMw[2][2] = -s23 * c4 * s5 + c23 * c5;
848 
849  fMw[0][3] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
850  fMw[1][3] = s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
851  fMw[2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
852 
853  // std::cout << "Wrist position fMw: " << std::endl << fMw;
854 
855  return;
856 }
857 
869 {
870  // Set the rotation as identity
871  wMe.eye();
872 
873  // Set the translation
874  wMe[2][3] = d6;
875 }
876 
888 void vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const { eMc_ = this->eMc; }
889 
900 {
901  eMs.eye();
902  eMs[2][3] = -d7; // tz = -d7
903 }
904 
916 void vpViper::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->eMc.inverse(); }
917 
933 {
935  get_cMe(cMe);
936 
937  cVe.build(cMe);
938 
939  return;
940 }
941 
964 void vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
965 {
966  vpMatrix V(6, 6);
967  V = 0;
968  // Compute the first and last block of V
970  get_fMw(q, fMw);
971  vpRotationMatrix fRw;
972  fMw.extract(fRw);
973  vpRotationMatrix wRf;
974  wRf = fRw.inverse();
975  for (unsigned int i = 0; i < 3; i++) {
976  for (unsigned int j = 0; j < 3; j++) {
977  V[i][j] = V[i + 3][j + 3] = wRf[i][j];
978  }
979  }
980  // Compute the second block of V
982  get_wMe(wMe);
984  eMw = wMe.inverse();
986  eMw.extract(etw);
987  vpMatrix block2 = etw.skew() * wRf;
988  for (unsigned int i = 0; i < 3; i++) {
989  for (unsigned int j = 0; j < 3; j++) {
990  V[i][j + 3] = block2[i][j];
991  }
992  }
993  // Compute eJe
994  vpMatrix fJw;
995  get_fJw(q, fJw);
996  eJe = V * fJw;
997 
998  return;
999 }
1000 
1048 void vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
1049 {
1050  double q1 = q[0];
1051  double q2 = q[1];
1052  double q3 = q[2];
1053  double q4 = q[3];
1054  double q5 = q[4];
1055 
1056  double c1 = cos(q1);
1057  double s1 = sin(q1);
1058  double c2 = cos(q2);
1059  double s2 = sin(q2);
1060  double c3 = cos(q3);
1061  double s3 = sin(q3);
1062  double c4 = cos(q4);
1063  double s4 = sin(q4);
1064  double c5 = cos(q5);
1065  double s5 = sin(q5);
1066  double c23 = cos(q2 + q3);
1067  double s23 = sin(q2 + q3);
1068 
1069  vpColVector J1(6);
1070  vpColVector J2(6);
1071  vpColVector J3(6);
1072  vpColVector J4(6);
1073  vpColVector J5(6);
1074  vpColVector J6(6);
1075 
1076  // Jacobian when d6 is set to zero
1077  J1[0] = -s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
1078  J1[1] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
1079  J1[2] = 0;
1080  J1[3] = 0;
1081  J1[4] = 0;
1082  J1[5] = 1;
1083 
1084  J2[0] = c1 * (s23 * a3 + c23 * d4 - a2 * s2);
1085  J2[1] = s1 * (s23 * a3 + c23 * d4 - a2 * s2);
1086  J2[2] = c23 * a3 - s23 * d4 - a2 * c2;
1087  J2[3] = -s1;
1088  J2[4] = c1;
1089  J2[5] = 0;
1090 
1091  J3[0] = c1 * (a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) * d4);
1092  J3[1] = s1 * (a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) * d4);
1093  J3[2] = -a3 * (s2 * s3 - c2 * c3) - d4 * (s2 * c3 + c2 * s3);
1094  J3[3] = -s1;
1095  J3[4] = c1;
1096  J3[5] = 0;
1097 
1098  J4[0] = 0;
1099  J4[1] = 0;
1100  J4[2] = 0;
1101  J4[3] = c1 * s23;
1102  J4[4] = s1 * s23;
1103  J4[5] = c23;
1104 
1105  J5[0] = 0;
1106  J5[1] = 0;
1107  J5[2] = 0;
1108  J5[3] = -c23 * c1 * s4 - s1 * c4;
1109  J5[4] = c1 * c4 - c23 * s1 * s4;
1110  J5[5] = s23 * s4;
1111 
1112  J6[0] = 0;
1113  J6[1] = 0;
1114  J6[2] = 0;
1115  J6[3] = (c1 * c23 * c4 - s1 * s4) * s5 + c1 * s23 * c5;
1116  J6[4] = (s1 * c23 * c4 + c1 * s4) * s5 + s1 * s23 * c5;
1117  J6[5] = -s23 * c4 * s5 + c23 * c5;
1118 
1119  fJw.resize(6, 6);
1120  for (unsigned int i = 0; i < 6; i++) {
1121  fJw[i][0] = J1[i];
1122  fJw[i][1] = J2[i];
1123  fJw[i][2] = J3[i];
1124  fJw[i][3] = J4[i];
1125  fJw[i][4] = J5[i];
1126  fJw[i][5] = J6[i];
1127  }
1128  return;
1129 }
1153 void vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1154 {
1155  vpMatrix V(6, 6);
1156  V = 0;
1157  // Set the first and last block to identity
1158  for (unsigned int i = 0; i < 6; i++)
1159  V[i][i] = 1;
1160 
1161  // Compute the second block of V
1162  vpHomogeneousMatrix fMw;
1163  get_fMw(q, fMw);
1164  vpRotationMatrix fRw;
1165  fMw.extract(fRw);
1166  vpHomogeneousMatrix wMe;
1167  get_wMe(wMe);
1168  vpHomogeneousMatrix eMw;
1169  eMw = wMe.inverse();
1170  vpTranslationVector etw;
1171  eMw.extract(etw);
1172  vpMatrix block2 = (fRw * etw).skew();
1173  // Set the second block
1174  for (unsigned int i = 0; i < 3; i++)
1175  for (unsigned int j = 0; j < 3; j++)
1176  V[i][j + 3] = block2[i][j];
1177 
1178  // Compute fJe
1179  vpMatrix fJw;
1180  get_fJw(q, fJw);
1181  fJe = V * fJw;
1182 
1183  return;
1184 }
1185 
1194 
1203 
1214 double vpViper::getCoupl56() const { return c56; }
1215 
1225 {
1226  this->eMc = eMc_;
1227  this->eMc.extract(etc);
1228  vpRotationMatrix R(this->eMc);
1229  this->erc.build(R);
1230 }
1231 
1242 void vpViper::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_)
1243 {
1244  this->etc = etc_;
1245  this->erc = erc_;
1246  vpRotationMatrix eRc(erc);
1247  this->eMc.build(etc, eRc);
1248 }
1249 
1259 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpViper &viper)
1260 {
1261  vpRotationMatrix eRc;
1262  viper.eMc.extract(eRc);
1263  vpRxyzVector rxyz(eRc);
1264 
1265  // Convert joint limits in degrees
1266  vpColVector jmax = viper.joint_max;
1267  vpColVector jmin = viper.joint_min;
1268  jmax.rad2deg();
1269  jmin.rad2deg();
1270 
1271  os << "Joint Max (deg):" << std::endl
1272  << "\t" << jmax.t() << std::endl
1273 
1274  << "Joint Min (deg): " << std::endl
1275  << "\t" << jmin.t() << std::endl
1276 
1277  << "Coupling 5-6:" << std::endl
1278  << "\t" << viper.c56 << std::endl
1279 
1280  << "eMc: " << std::endl
1281  << "\tTranslation (m): " << viper.eMc[0][3] << " " << viper.eMc[1][3] << " " << viper.eMc[2][3] << "\t"
1282  << std::endl
1283  << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1284  << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1285  << "\t" << std::endl;
1286 
1287  return os;
1288 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:354
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:603
unsigned int getRows() const
Definition: vpArray2D.h:339
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector & rad2deg()
Definition: vpColVector.h:972
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
static double rad(double deg)
Definition: vpMath.h:127
static double sqr(double x)
Definition: vpMath.h:201
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix inverse() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:177
vpRxyzVector & build(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
Modelization of the ADEPT Viper robot.
Definition: vpViper.h:111
double getCoupl56() const
Definition: vpViper.cpp:1214
void get_fJw(const vpColVector &q, vpMatrix &fJw) const
Definition: vpViper.cpp:1048
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1224
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpViper.cpp:118
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
Definition: vpViper.cpp:809
vpTranslationVector etc
Definition: vpViper.h:156
double d6
for joint 6
Definition: vpViper.h:164
double a3
for joint 3
Definition: vpViper.h:162
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:916
double d4
for joint 4
Definition: vpViper.h:163
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1153
vpColVector joint_max
Definition: vpViper.h:169
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:567
double c56
Mechanical coupling between joint 5 and joint 6.
Definition: vpViper.h:166
vpColVector getJointMin() const
Definition: vpViper.cpp:1193
void get_eMc(vpHomogeneousMatrix &eMc) const
Definition: vpViper.cpp:888
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:154
double a1
Definition: vpViper.h:160
vpRxyzVector erc
Definition: vpViper.h:157
vpColVector getJointMax() const
Definition: vpViper.cpp:1202
vpColVector joint_min
Definition: vpViper.h:170
void get_eMs(vpHomogeneousMatrix &eMs) const
Definition: vpViper.cpp:899
double a2
for joint 2
Definition: vpViper.h:161
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:151
vpViper()
Definition: vpViper.cpp:62
void get_wMe(vpHomogeneousMatrix &wMe) const
Definition: vpViper.cpp:868
unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:219
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:964
double d1
for joint 1
Definition: vpViper.h:160
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:719
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpViper.cpp:932
double d7
for force/torque location
Definition: vpViper.h:165
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:604