Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
55const 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
140bool 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 } else {
261 double a = sqrt(a_2);
262 c1[0] = px / a;
263 s1[0] = py / a;
264 c1[4] = -px / a;
265 s1[4] = -py / a;
266 }
267
268 double q1_mod;
269 for (unsigned int i = 0; i < 8; i += 4) {
270 q_sol[i][0] = atan2(s1[i], c1[i]);
271 if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) == true) {
272 q_sol[i][0] = q1_mod;
273 for (unsigned int j = 1; j < 4; j++) {
274 c1[i + j] = c1[i];
275 s1[i + j] = s1[i];
276 q_sol[i + j][0] = q_sol[i][0];
277 }
278 } else {
279 for (unsigned int j = 1; j < 4; j++)
280 ok[i + j] = false;
281 }
282 }
283
284 // Compute q3
285 double K, q3_mod;
286 for (unsigned int i = 0; i < 8; i += 4) {
287 if (ok[i] == true) {
288 K = (px * px + py * py + pz * pz + a1 * a1 - a2 * a2 - a3 * a3 + d1 * d1 - d4 * d4 -
289 2 * (a1 * c1[i] * px + a1 * s1[i] * py + d1 * pz)) /
290 (2 * a2);
291 double d4_a3_K = d4 * d4 + a3 * a3 - K * K;
292
293 q_sol[i][2] = atan2(a3, d4) + atan2(K, sqrt(d4_a3_K));
294 q_sol[i + 2][2] = atan2(a3, d4) + atan2(K, -sqrt(d4_a3_K));
295
296 for (unsigned int j = 0; j < 4; j += 2) {
297 if (d4_a3_K < 0) {
298 for (unsigned int k = 0; k < 2; k++)
299 ok[i + j + k] = false;
300 } else {
301 if (convertJointPositionInLimits(2, q_sol[i + j][2], q3_mod, verbose) == true) {
302 for (unsigned int k = 0; k < 2; k++) {
303 q_sol[i + j + k][2] = q3_mod;
304 c3[i + j + k] = cos(q3_mod);
305 s3[i + j + k] = sin(q3_mod);
306 }
307 } else {
308 for (unsigned int k = 0; k < 2; k++)
309 ok[i + j + k] = false;
310 }
311 }
312 }
313 }
314 }
315 // std::cout << "ok apres q3: ";
316 // for (unsigned int i=0; i< 8; i++)
317 // std::cout << ok[i] << " ";
318 // std::cout << std::endl;
319
320 // Compute q2
321 double q23[8], q2_mod;
322 for (unsigned int i = 0; i < 8; i += 2) {
323 if (ok[i] == true) {
324 // Compute q23 = q2+q3
325 c23[i] = (-(a3 - a2 * c3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (d4 + a2 * s3[i])) /
326 ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
327 s23[i] = ((d4 + a2 * s3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (a3 - a2 * c3[i])) /
328 ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
329 q23[i] = atan2(s23[i], c23[i]);
330 // std::cout << i << " c23 = " << c23[i] << " s23 = " << s23[i] <<
331 // std::endl;
332 // q2 = q23 - q3
333 q_sol[i][1] = q23[i] - q_sol[i][2];
334
335 if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) == true) {
336 for (unsigned int j = 0; j < 2; j++) {
337 q_sol[i + j][1] = q2_mod;
338 c23[i + j] = c23[i];
339 s23[i + j] = s23[i];
340 }
341 } else {
342 for (unsigned int j = 0; j < 2; j++)
343 ok[i + j] = false;
344 }
345 }
346 }
347 // std::cout << "ok apres q2: ";
348 // for (unsigned int i=0; i< 8; i++)
349 // std::cout << ok[i] << " ";
350 // std::cout << std::endl;
351
352 // Compute q4 as long as s5 != 0
353 double r13 = fMw[0][2];
354 double r23 = fMw[1][2];
355 double r33 = fMw[2][2];
356 double s4s5, c4s5, q4_mod, q5_mod;
357 for (unsigned int i = 0; i < 8; i += 2) {
358 if (ok[i] == true) {
359 s4s5 = -s1[i] * r13 + c1[i] * r23;
360 c4s5 = c1[i] * c23[i] * r13 + s1[i] * c23[i] * r23 - s23[i] * r33;
361 if (fabs(s4s5) < vpMath::rad(0.5) && fabs(c4s5) < vpMath::rad(0.5)) {
362 // s5 = 0
363 c5[i] = c1[i] * s23[i] * r13 + s1[i] * s23[i] * r23 + c23[i] * r33;
364 // std::cout << "Singularity: s5 near 0: ";
365 if (c5[i] > 0.)
366 q_sol[i][4] = 0.0;
367 else
368 q_sol[i][4] = M_PI;
369
370 if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) == true) {
371 for (unsigned int j = 0; j < 2; j++) {
372 q_sol[i + j][3] = q[3]; // keep current q4
373 q_sol[i + j][4] = q5_mod;
374 c4[i] = cos(q_sol[i + j][3]);
375 s4[i] = sin(q_sol[i + j][3]);
376 }
377 } else {
378 for (unsigned int j = 0; j < 2; j++)
379 ok[i + j] = false;
380 }
381 } else {
382// s5 != 0
383#if 0 // Modified 2016/03/10 since if and else are the same
384 // if (c4s5 == 0) {
385 if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
386 // c4 = 0
387 // vpTRACE("c4 = 0");
388 // q_sol[i][3] = q[3]; // keep current position
389 q_sol[i][3] = atan2(s4s5, c4s5);
390 }
391 else {
392 q_sol[i][3] = atan2(s4s5, c4s5);
393 }
394#else
395 q_sol[i][3] = atan2(s4s5, c4s5);
396#endif
397 if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) == true) {
398 q_sol[i][3] = q4_mod;
399 c4[i] = cos(q4_mod);
400 s4[i] = sin(q4_mod);
401 } else {
402 ok[i] = false;
403 }
404 if (q_sol[i][3] > 0.)
405 q_sol[i + 1][3] = q_sol[i][3] + M_PI;
406 else
407 q_sol[i + 1][3] = q_sol[i][3] - M_PI;
408 if (convertJointPositionInLimits(3, q_sol[i + 1][3], q4_mod, verbose) == true) {
409 q_sol[i + 1][3] = q4_mod;
410 c4[i + 1] = cos(q4_mod);
411 s4[i + 1] = sin(q4_mod);
412 } else {
413 ok[i + 1] = false;
414 }
415
416 // Compute q5
417 for (unsigned int j = 0; j < 2; j++) {
418 if (ok[i + j] == true) {
419 c5[i + j] = c1[i + j] * s23[i + j] * r13 + s1[i + j] * s23[i + j] * r23 + c23[i + j] * r33;
420 s5[i + j] = (c1[i + j] * c23[i + j] * c4[i + j] - s1[i + j] * s4[i + j]) * r13 +
421 (s1[i + j] * c23[i + j] * c4[i + j] + c1[i + j] * s4[i + j]) * r23 -
422 s23[i + j] * c4[i + j] * r33;
423
424 q_sol[i + j][4] = atan2(s5[i + j], c5[i + j]);
425 if (convertJointPositionInLimits(4, q_sol[i + j][4], q5_mod, verbose) == true) {
426 q_sol[i + j][4] = q5_mod;
427 } else {
428
429 ok[i + j] = false;
430 }
431 }
432 }
433 }
434 }
435 }
436
437 // Compute q6
438 // 4 solutions for q6 and 4 more solutions by flipping the wrist (see below)
439 double r12 = fMw[0][1];
440 double r22 = fMw[1][1];
441 double r32 = fMw[2][1];
442 double q6_mod;
443 for (unsigned int i = 0; i < 8; i++) {
444 c6[i] = -(c1[i] * c23[i] * s4[i] + s1[i] * c4[i]) * r12 + (c1[i] * c4[i] - s1[i] * c23[i] * s4[i]) * r22 +
445 s23[i] * s4[i] * r32;
446 s6[i] = -(c1[i] * c23[i] * c4[i] * c5[i] - c1[i] * s23[i] * s5[i] - s1[i] * s4[i] * c5[i]) * r12 -
447 (s1[i] * c23[i] * c4[i] * c5[i] - s1[i] * s23[i] * s5[i] + c1[i] * s4[i] * c5[i]) * r22 +
448 (c23[i] * s5[i] + s23[i] * c4[i] * c5[i]) * r32;
449
450 q_sol[i][5] = atan2(s6[i], c6[i]);
451 if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) == true) {
452 q_sol[i][5] = q6_mod;
453 } else {
454 ok[i] = false;
455 }
456 }
457
458 // Select the best config in terms of distance from the current position
459 unsigned int nbsol = 0;
460 unsigned int sol = 0;
461 vpColVector dist(8);
462 for (unsigned int i = 0; i < 8; i++) {
463 if (ok[i] == true) {
464 nbsol++;
465 sol = i;
466 vpColVector weight(6);
467 weight = 1;
468 weight[0] = 8;
469 weight[1] = weight[2] = 4;
470 dist[i] = 0;
471 for (unsigned int j = 0; j < 6; j++) {
472 double rought_dist = q[j] - q_sol[i][j];
473 double modulo_dist = rought_dist;
474 if (rought_dist > 0) {
475 if (fabs(rought_dist - 2 * M_PI) < fabs(rought_dist))
476 modulo_dist = rought_dist - 2 * M_PI;
477 } else {
478 if (fabs(rought_dist + 2 * M_PI) < fabs(rought_dist))
479 modulo_dist = rought_dist + 2 * M_PI;
480 }
481
482 dist[i] += weight[j] * vpMath::sqr(modulo_dist);
483 }
484 }
485
486 }
487 // std::cout << "dist: " << dist.t() << std::endl;
488 if (nbsol) {
489 for (unsigned int i = 0; i < 8; i++) {
490 if (ok[i] == true)
491 if (dist[i] < dist[sol])
492 sol = i;
493 }
494 // Update the inverse kinematics solution
495 q = q_sol[sol];
496
497 }
498 return nbsol;
499}
500
555unsigned int vpViper::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose) const
556{
560 this->get_wMe(wMe);
561 this->get_eMc(eMc_);
562 fMw = fMc * eMc_.inverse() * wMe.inverse();
563
564 return (getInverseKinematicsWrist(fMw, q, verbose));
565}
566
593{
595 get_fMc(q, fMc);
596
597 return fMc;
598}
599
622{
623
624 // Compute the direct geometric model: fMe = transformation between
625 // fix and end effector frame.
627
628 get_fMe(q, fMe);
629
630 fMc = fMe * this->eMc;
631
632 return;
633}
634
708{
709 double q1 = q[0];
710 double q2 = q[1];
711 double q3 = q[2];
712 double q4 = q[3];
713 double q5 = q[4];
714 double q6 = q[5];
715 // We turn off the coupling since the measured positions are joint position
716 // taking into account the coupling factor. The coupling factor is relevant
717 // if positions are motor position.
718 // double q6 = q[5] + c56 * q[4];
719
720 double c1 = cos(q1);
721 double s1 = sin(q1);
722 double c2 = cos(q2);
723 double s2 = sin(q2);
724 // double c3 = cos(q3);
725 // double s3 = sin(q3);
726 double c4 = cos(q4);
727 double s4 = sin(q4);
728 double c5 = cos(q5);
729 double s5 = sin(q5);
730 double c6 = cos(q6);
731 double s6 = sin(q6);
732 double c23 = cos(q2 + q3);
733 double s23 = sin(q2 + q3);
734
735 fMe[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
736 fMe[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
737 fMe[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
738
739 fMe[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
740 fMe[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
741 fMe[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
742
743 fMe[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
744 fMe[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
745 fMe[2][2] = -s23 * c4 * s5 + c23 * c5;
746
747 fMe[0][3] = c1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) - s1 * s4 * s5 * d6;
748 fMe[1][3] = s1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) + c1 * s4 * s5 * d6;
749 fMe[2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;
750
751 // std::cout << "Effector position fMe: " << std::endl << fMe;
752
753 return;
754}
798{
799 double q1 = q[0];
800 double q2 = q[1];
801 double q3 = q[2];
802 double q4 = q[3];
803 double q5 = q[4];
804 double q6 = q[5];
805 // We turn off the coupling since the measured positions are joint position
806 // taking into account the coupling factor. The coupling factor is relevant
807 // if positions are motor position.
808 // double q6 = q[5] + c56 * q[4];
809
810 double c1 = cos(q1);
811 double s1 = sin(q1);
812 double c2 = cos(q2);
813 double s2 = sin(q2);
814 // double c3 = cos(q3);
815 // double s3 = sin(q3);
816 double c4 = cos(q4);
817 double s4 = sin(q4);
818 double c5 = cos(q5);
819 double s5 = sin(q5);
820 double c6 = cos(q6);
821 double s6 = sin(q6);
822 double c23 = cos(q2 + q3);
823 double s23 = sin(q2 + q3);
824
825 fMw[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
826 fMw[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
827 fMw[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
828
829 fMw[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
830 fMw[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
831 fMw[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
832
833 fMw[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
834 fMw[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
835 fMw[2][2] = -s23 * c4 * s5 + c23 * c5;
836
837 fMw[0][3] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
838 fMw[1][3] = s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
839 fMw[2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
840
841 // std::cout << "Wrist position fMw: " << std::endl << fMw;
842
843 return;
844}
845
857{
858 // Set the rotation as identity
859 wMe.eye();
860
861 // Set the translation
862 wMe[2][3] = d6;
863}
864
876void vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const { eMc_ = this->eMc; }
877
888{
889 eMs.eye();
890 eMs[2][3] = -d7; // tz = -d7
891}
892
904void vpViper::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->eMc.inverse(); }
905
921{
923 get_cMe(cMe);
924
925 cVe.buildFrom(cMe);
926
927 return;
928}
929
952void vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
953{
954 vpMatrix V(6, 6);
955 V = 0;
956 // Compute the first and last block of V
958 get_fMw(q, fMw);
960 fMw.extract(fRw);
962 wRf = fRw.inverse();
963 for (unsigned int i = 0; i < 3; i++) {
964 for (unsigned int j = 0; j < 3; j++) {
965 V[i][j] = V[i + 3][j + 3] = wRf[i][j];
966 }
967 }
968 // Compute the second block of V
970 get_wMe(wMe);
972 eMw = wMe.inverse();
974 eMw.extract(etw);
975 vpMatrix block2 = etw.skew() * wRf;
976 for (unsigned int i = 0; i < 3; i++) {
977 for (unsigned int j = 0; j < 3; j++) {
978 V[i][j + 3] = block2[i][j];
979 }
980 }
981 // Compute eJe
982 vpMatrix fJw;
983 get_fJw(q, fJw);
984 eJe = V * fJw;
985
986 return;
987}
988
1036void vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
1037{
1038 double q1 = q[0];
1039 double q2 = q[1];
1040 double q3 = q[2];
1041 double q4 = q[3];
1042 double q5 = q[4];
1043
1044 double c1 = cos(q1);
1045 double s1 = sin(q1);
1046 double c2 = cos(q2);
1047 double s2 = sin(q2);
1048 double c3 = cos(q3);
1049 double s3 = sin(q3);
1050 double c4 = cos(q4);
1051 double s4 = sin(q4);
1052 double c5 = cos(q5);
1053 double s5 = sin(q5);
1054 double c23 = cos(q2 + q3);
1055 double s23 = sin(q2 + q3);
1056
1057 vpColVector J1(6);
1058 vpColVector J2(6);
1059 vpColVector J3(6);
1060 vpColVector J4(6);
1061 vpColVector J5(6);
1062 vpColVector J6(6);
1063
1064 // Jacobian when d6 is set to zero
1065 J1[0] = -s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
1066 J1[1] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
1067 J1[2] = 0;
1068 J1[3] = 0;
1069 J1[4] = 0;
1070 J1[5] = 1;
1071
1072 J2[0] = c1 * (s23 * a3 + c23 * d4 - a2 * s2);
1073 J2[1] = s1 * (s23 * a3 + c23 * d4 - a2 * s2);
1074 J2[2] = c23 * a3 - s23 * d4 - a2 * c2;
1075 J2[3] = -s1;
1076 J2[4] = c1;
1077 J2[5] = 0;
1078
1079 J3[0] = c1 * (a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) * d4);
1080 J3[1] = s1 * (a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) * d4);
1081 J3[2] = -a3 * (s2 * s3 - c2 * c3) - d4 * (s2 * c3 + c2 * s3);
1082 J3[3] = -s1;
1083 J3[4] = c1;
1084 J3[5] = 0;
1085
1086 J4[0] = 0;
1087 J4[1] = 0;
1088 J4[2] = 0;
1089 J4[3] = c1 * s23;
1090 J4[4] = s1 * s23;
1091 J4[5] = c23;
1092
1093 J5[0] = 0;
1094 J5[1] = 0;
1095 J5[2] = 0;
1096 J5[3] = -c23 * c1 * s4 - s1 * c4;
1097 J5[4] = c1 * c4 - c23 * s1 * s4;
1098 J5[5] = s23 * s4;
1099
1100 J6[0] = 0;
1101 J6[1] = 0;
1102 J6[2] = 0;
1103 J6[3] = (c1 * c23 * c4 - s1 * s4) * s5 + c1 * s23 * c5;
1104 J6[4] = (s1 * c23 * c4 + c1 * s4) * s5 + s1 * s23 * c5;
1105 J6[5] = -s23 * c4 * s5 + c23 * c5;
1106
1107 fJw.resize(6, 6);
1108 for (unsigned int i = 0; i < 6; i++) {
1109 fJw[i][0] = J1[i];
1110 fJw[i][1] = J2[i];
1111 fJw[i][2] = J3[i];
1112 fJw[i][3] = J4[i];
1113 fJw[i][4] = J5[i];
1114 fJw[i][5] = J6[i];
1115 }
1116 return;
1117}
1141void vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1142{
1143 vpMatrix V(6, 6);
1144 V = 0;
1145 // Set the first and last block to identity
1146 for (unsigned int i = 0; i < 6; i++)
1147 V[i][i] = 1;
1148
1149 // Compute the second block of V
1151 get_fMw(q, fMw);
1152 vpRotationMatrix fRw;
1153 fMw.extract(fRw);
1155 get_wMe(wMe);
1157 eMw = wMe.inverse();
1159 eMw.extract(etw);
1160 vpMatrix block2 = (fRw * etw).skew();
1161 // Set the second block
1162 for (unsigned int i = 0; i < 3; i++)
1163 for (unsigned int j = 0; j < 3; j++)
1164 V[i][j + 3] = block2[i][j];
1165
1166 // Compute fJe
1167 vpMatrix fJw;
1168 get_fJw(q, fJw);
1169 fJe = V * fJw;
1170
1171 return;
1172}
1173
1182
1191
1202double vpViper::getCoupl56() const { return c56; }
1203
1213{
1214 this->eMc = eMc_;
1215 this->eMc.extract(etc);
1216 vpRotationMatrix R(this->eMc);
1217 this->erc.buildFrom(R);
1218}
1219
1231{
1232 this->etc = etc_;
1233 this->erc = erc_;
1234 vpRotationMatrix eRc(erc);
1235 this->eMc.buildFrom(etc, eRc);
1236}
1237
1247VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpViper &viper)
1248{
1249 vpRotationMatrix eRc;
1250 viper.eMc.extract(eRc);
1251 vpRxyzVector rxyz(eRc);
1252
1253 // Convert joint limits in degrees
1254 vpColVector jmax = viper.joint_max;
1255 vpColVector jmin = viper.joint_min;
1256 jmax.rad2deg();
1257 jmin.rad2deg();
1258
1259 os << "Joint Max (deg):" << std::endl
1260 << "\t" << jmax.t() << std::endl
1261
1262 << "Joint Min (deg): " << std::endl
1263 << "\t" << jmin.t() << std::endl
1264
1265 << "Coupling 5-6:" << std::endl
1266 << "\t" << viper.c56 << std::endl
1267
1268 << "eMc: " << std::endl
1269 << "\tTranslation (m): " << viper.eMc[0][3] << " " << viper.eMc[1][3] << " " << viper.eMc[2][3] << "\t"
1270 << std::endl
1271 << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1272 << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1273 << "\t" << std::endl;
1274
1275 return os;
1276}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
vpColVector & rad2deg()
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
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:116
static double sqr(double x)
Definition vpMath.h:124
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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.
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:111
double getCoupl56() const
Definition vpViper.cpp:1202
void get_fJw(const vpColVector &q, vpMatrix &fJw) const
Definition vpViper.cpp:1036
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition vpViper.cpp:1212
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition vpViper.cpp:118
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
Definition vpViper.cpp:797
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:904
double d4
for joint 4
Definition vpViper.h:163
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition vpViper.cpp:1141
vpColVector joint_max
Definition vpViper.h:169
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition vpViper.cpp:555
double c56
Mechanical coupling between joint 5 and joint 6.
Definition vpViper.h:166
vpColVector getJointMin() const
Definition vpViper.cpp:1181
void get_eMc(vpHomogeneousMatrix &eMc) const
Definition vpViper.cpp:876
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:1190
vpColVector joint_min
Definition vpViper.h:170
void get_eMs(vpHomogeneousMatrix &eMs) const
Definition vpViper.cpp:887
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:856
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:952
double d1
for joint 1
Definition vpViper.h:160
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition vpViper.cpp:707
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition vpViper.cpp:920
double d7
for force/torque location
Definition vpViper.h:165
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition vpViper.cpp:592