Visual Servoing Platform version 3.5.0
vpAfma4.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 the Irisa's Afma4 robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
47#include <visp3/core/vpCameraParameters.h>
48#include <visp3/core/vpDebug.h>
49#include <visp3/core/vpRotationMatrix.h>
50#include <visp3/core/vpRxyzVector.h>
51#include <visp3/core/vpTranslationVector.h>
52#include <visp3/core/vpVelocityTwistMatrix.h>
53#include <visp3/robot/vpAfma4.h>
54#include <visp3/robot/vpRobotException.h>
55
56/* ----------------------------------------------------------------------- */
57/* --- STATIC ------------------------------------------------------------ */
58/* ---------------------------------------------------------------------- */
59const unsigned int vpAfma4::njoint = 4;
60
66vpAfma4::vpAfma4() : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
67{
68 // Set the default parameters in case of the config files are not available.
69
70 //
71 // Geometric model constant parameters
72 //
73 this->_a1 = 0.205; // distance along x2
74 this->_d3 = 0.403; // distance along z2
75 this->_d4 = 0.14; // distance along z3
76
77 // Maximal value of the joints
78 this->_joint_max[0] = 1.8; // rad
79 this->_joint_max[1] = 0.9; // meter
80 this->_joint_max[2] = 0.9; // rad
81 this->_joint_max[3] = 0.76; // rad
82 // Minimal value of the joints
83 this->_joint_min[0] = -1.5; // rad
84 this->_joint_min[1] = -0.9; // meter
85 this->_joint_min[2] = -3.5; // rad
86 this->_joint_min[3] = -0.76; // rad
87
88 // Camera extrinsic parameters: effector to camera frame
89 this->_etc[0] = 0.; // Translation
90 this->_etc[1] = 0.;
91 this->_etc[2] = 0.;
92 this->_erc[0] = 0.; // Rotation
93 this->_erc[1] = -M_PI / 2.;
94 this->_erc[2] = 0;
95
97 this->_eMc.buildFrom(_etc, eRc);
98
99 init();
100}
101
106void vpAfma4::init(void) { return; }
107
140{
142 fMc = get_fMc(q);
143
144 return fMc;
145}
146
177{
179 get_fMc(q, fMc);
180
181 return fMc;
182}
183
211{
212
213 // Compute the direct geometric model: fMe = transformation between
214 // fix and end effector frame.
216
217 get_fMe(q, fMe);
218
219 fMc = fMe * this->_eMc;
220
221 return;
222}
223
260{
261 double q1 = q[0]; // rot touret
262 double q2 = q[1]; // vertical translation
263 double q4 = q[2]; // pan
264 double q5 = q[3]; // tilt
265
266 double c1 = cos(q1);
267 double s1 = sin(q1);
268 double c4 = cos(q4);
269 double s4 = sin(q4);
270 double c5 = cos(q5);
271 double s5 = sin(q5);
272
273 /* Calcul du modele d'apres les angles. */
274 fMe[0][0] = c1 * s4 * c5 + s1 * c4 * c5;
275 fMe[0][1] = -c1 * s4 * s5 - s1 * c4 * s5;
276 fMe[0][2] = c1 * c4 - s1 * s4;
277 fMe[0][3] = c1 * this->_a1 - s1 * (this->_d3);
278
279 fMe[1][0] = s1 * s4 * c5 - c1 * c4 * c5;
280 fMe[1][1] = -s1 * s4 * s5 + c1 * c4 * s5;
281 fMe[1][2] = s1 * c4 + c1 * s4;
282 fMe[1][3] = s1 * this->_a1 + c1 * (this->_d3);
283
284 fMe[2][0] = -s5;
285 fMe[2][1] = -c5;
286 fMe[2][2] = 0.f;
287 fMe[2][3] = this->_d4 + q2;
288
289 fMe[3][0] = 0.f;
290 fMe[3][1] = 0.f;
291 fMe[3][2] = 0.f;
292 fMe[3][3] = 1;
293
294 // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
295
296 return;
297}
298
309void vpAfma4::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
310
321{
323 get_cMe(cMe);
324
325 cVe.buildFrom(cMe);
326
327 return;
328}
329
349{
350 vpHomogeneousMatrix fMc, cMf;
351 get_fMc(q, fMc);
352 cMf = fMc.inverse();
353
354 cVf.buildFrom(cMf);
355
356 return;
357}
358
395void vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
396{
397 double q4 = q[2]; // pan
398 double q5 = q[3]; // tilt
399
400 double c4 = cos(q4);
401 double s4 = sin(q4);
402 double c5 = cos(q5);
403 double s5 = sin(q5);
404
405 eJe.resize(6, 4);
406
407 eJe = 0;
408
409 eJe[0][0] = -(this->_a1 * c4 + this->_d3 * s4) * c5;
410 eJe[0][1] = -s5;
411 eJe[1][0] = (this->_a1 * c4 + this->_d3 * s4) * s5;
412 eJe[1][1] = -c5;
413 eJe[2][0] = (this->_a1 * s4 - this->_d3 * c4);
414 eJe[3][0] = eJe[3][2] = -s5;
415 eJe[4][0] = eJe[4][2] = -c5;
416 eJe[5][3] = 1.;
417}
418
450void vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
451{
452 fJe.resize(6, 4);
453
454 double q1 = q[0]; // rot touret
455 double q4 = q[2]; // pan
456
457 double c1 = cos(q1);
458 double s1 = sin(q1);
459 double c14 = cos(q1 + q4);
460 double s14 = sin(q1 + q4);
461
462 fJe = 0;
463
464 fJe[0][0] = -s1 * this->_a1 - c1 * this->_d3;
465
466 fJe[1][0] = c1 * this->_a1 - s1 * this->_d3;
467
468 fJe[2][1] = 1.0;
469
470 fJe[3][3] = c14;
471
472 fJe[4][3] = s14;
473
474 fJe[5][0] = fJe[5][2] = 1.0;
475}
476
506void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
507{
508 fJe_inverse.resize(4, 6);
509 fJe_inverse = 0;
510
511 double q1 = q[0]; // rot touret
512 double q4 = q[2]; // pan
513
514 double c1 = cos(q1);
515 double s1 = sin(q1);
516 double c14 = cos(q1 + q4);
517 double s14 = sin(q1 + q4);
518
519 double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;
520
521 fJe_inverse[0][0] = (-s1 * this->_a1 - c1 * this->_d3) / det;
522 fJe_inverse[0][1] = (c1 * this->_a1 - s1 * this->_d3) / det;
523
524 fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;
525
526 fJe_inverse[2][0] = -fJe_inverse[0][0];
527 fJe_inverse[2][1] = -fJe_inverse[0][1];
528
529 fJe_inverse[3][3] = c14;
530 fJe_inverse[3][4] = s14;
531}
532
542{
543 vpColVector qmin(4);
544 for (unsigned int i = 0; i < 4; i++)
545 qmin[i] = this->_joint_min[i];
546 return qmin;
547}
548
558{
559 vpColVector qmax(4);
560 for (unsigned int i = 0; i < 4; i++)
561 qmax[i] = this->_joint_max[i];
562 return qmax;
563}
564
574VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4)
575{
577 afma4._eMc.extract(eRc);
578 vpRxyzVector rxyz(eRc);
579
580 os << "Joint Max:" << std::endl
581 << "\t" << afma4._joint_max[0] << "\t" << afma4._joint_max[1] << "\t" << afma4._joint_max[2] << "\t"
582 << afma4._joint_max[3] << "\t" << std::endl
583
584 << "Joint Min: " << std::endl
585 << "\t" << afma4._joint_min[0] << "\t" << afma4._joint_min[1] << "\t" << afma4._joint_min[2] << "\t"
586 << afma4._joint_min[3] << "\t" << std::endl
587
588 << "a1: " << std::endl
589 << "\t" << afma4._a1 << "\t" << std::endl
590
591 << "d3: " << std::endl
592 << "\t" << afma4._d3 << "\t" << std::endl
593
594 << "d4: " << std::endl
595 << "\t" << afma4._d4 << "\t" << std::endl
596
597 << "eMc: " << std::endl
598 << "\tTranslation (m): " << afma4._eMc[0][3] << " " << afma4._eMc[1][3] << " " << afma4._eMc[2][3] << "\t"
599 << std::endl
600 << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
601 << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
602 << "\t" << std::endl;
603
604 return os;
605}
Modelisation of Irisa's cylindrical robot named Afma4.
Definition: vpAfma4.h:111
double _d3
Definition: vpAfma4.h:147
vpColVector getJointMin() const
Definition: vpAfma4.cpp:541
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma4.cpp:450
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma4.cpp:176
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma4.cpp:309
vpTranslationVector _etc
Definition: vpAfma4.h:153
static const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:142
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma4.cpp:320
vpRxyzVector _erc
Definition: vpAfma4.h:154
double _a1
Definition: vpAfma4.h:146
void init(void)
Definition: vpAfma4.cpp:106
double _d4
Definition: vpAfma4.h:148
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma4.cpp:395
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition: vpAfma4.cpp:506
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition: vpAfma4.cpp:348
vpColVector getJointMax() const
Definition: vpAfma4.cpp:557
vpHomogeneousMatrix _eMc
Definition: vpAfma4.h:156
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma4.cpp:259
double _joint_min[4]
Definition: vpAfma4.h:150
double _joint_max[4]
Definition: vpAfma4.h:149
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma4.cpp:139
vpAfma4()
Definition: vpAfma4.cpp:66
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
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
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 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.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)