Visual Servoing Platform version 3.5.0
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
58const 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
143bool 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
563unsigned int vpViper::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose) const
564{
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
894void vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const { eMc_ = this->eMc; }
895
906{
907 eMs.eye();
908 eMs[2][3] = -d7; // tz = -d7
909}
910
922void 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
970void 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);
978 fMw.extract(fRw);
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
1054void 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}
1159void 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
1169 get_fMw(q, fMw);
1170 vpRotationMatrix fRw;
1171 fMw.extract(fRw);
1173 get_wMe(wMe);
1175 eMw = wMe.inverse();
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
1220double 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
1249{
1250 this->etc = etc_;
1251 this->erc = erc_;
1252 vpRotationMatrix eRc(erc);
1253 this->eMc.buildFrom(etc, eRc);
1254}
1255
1265VISP_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 resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
void rad2deg()
Definition: vpColVector.h:294
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
static double rad(double deg)
Definition: vpMath.h:110
static double sqr(double x)
Definition: vpMath.h:116
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
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:184
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Modelisation of the ADEPT Viper robot.
Definition: vpViper.h:114
double getCoupl56() const
Definition: vpViper.cpp:1220
void get_fJw(const vpColVector &q, vpMatrix &fJw) const
Definition: vpViper.cpp:1054
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1230
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpViper.cpp:121
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
Definition: vpViper.cpp:810
vpTranslationVector etc
Definition: vpViper.h:159
double d6
for joint 6
Definition: vpViper.h:167
double a3
for joint 3
Definition: vpViper.h:165
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
double d4
for joint 4
Definition: vpViper.h:166
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
vpColVector joint_max
Definition: vpViper.h:172
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
double c56
Mechanical coupling between joint 5 and joint 6.
Definition: vpViper.h:169
vpColVector getJointMin() const
Definition: vpViper.cpp:1199
void get_eMc(vpHomogeneousMatrix &eMc) const
Definition: vpViper.cpp:894
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:157
double a1
Definition: vpViper.h:163
vpRxyzVector erc
Definition: vpViper.h:160
vpColVector getJointMax() const
Definition: vpViper.cpp:1208
vpColVector joint_min
Definition: vpViper.h:173
void get_eMs(vpHomogeneousMatrix &eMs) const
Definition: vpViper.cpp:905
double a2
for joint 2
Definition: vpViper.h:164
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:154
vpViper()
Definition: vpViper.cpp:65
void get_wMe(vpHomogeneousMatrix &wMe) const
Definition: vpViper.cpp:874
unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:222
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
double d1
for joint 1
Definition: vpViper.h:163
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:715
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpViper.cpp:938
double d7
for force/torque location
Definition: vpViper.h:168
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600