Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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 /* ---------------------------------------------------------------------- */
59 const unsigned int vpAfma4::njoint = 4;
60 
66 vpAfma4::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 
106 void 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 
309 void 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 
395 void 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 
450 void 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 
506 void 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 
574 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4)
575 {
576  vpRotationMatrix eRc;
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 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
double _a1
Definition: vpAfma4.h:146
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma4.cpp:309
Implementation of an homogeneous matrix and operations on such kind of matrices.
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma4.cpp:259
vpHomogeneousMatrix _eMc
Definition: vpAfma4.h:156
double _d4
Definition: vpAfma4.h:148
vpHomogeneousMatrix inverse() const
void init(void)
Definition: vpAfma4.cpp:106
void extract(vpRotationMatrix &R) const
vpAfma4()
Definition: vpAfma4.cpp:66
Implementation of a rotation matrix and operations on such kind of matrices.
vpColVector getJointMin() const
Definition: vpAfma4.cpp:541
double _joint_max[4]
Definition: vpAfma4.h:149
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpTranslationVector _etc
Definition: vpAfma4.h:153
double _joint_min[4]
Definition: vpAfma4.h:150
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma4.cpp:450
static const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:142
Modelisation of Irisa&#39;s cylindrical robot named Afma4.
Definition: vpAfma4.h:110
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition: vpAfma4.cpp:506
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpAfma4 &afma4)
Definition: vpAfma4.cpp:574
vpRxyzVector _erc
Definition: vpAfma4.h:154
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma4.cpp:320
static double deg(double rad)
Definition: vpMath.h:103
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma4.cpp:176
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma4.cpp:395
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
double _d3
Definition: vpAfma4.h:147
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma4.cpp:139
vpColVector getJointMax() const
Definition: vpAfma4.cpp:557
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition: vpAfma4.cpp:348