Visual Servoing Platform  version 3.6.1 under development (2024-04-22)
vpAfma4.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 the Irisa's Afma4 robot.
33  *
34 *****************************************************************************/
35 
44 #include <visp3/core/vpCameraParameters.h>
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/core/vpRotationMatrix.h>
47 #include <visp3/core/vpRxyzVector.h>
48 #include <visp3/core/vpTranslationVector.h>
49 #include <visp3/core/vpVelocityTwistMatrix.h>
50 #include <visp3/robot/vpAfma4.h>
51 #include <visp3/robot/vpRobotException.h>
52 
53 /* ----------------------------------------------------------------------- */
54 /* --- STATIC ------------------------------------------------------------ */
55 /* ---------------------------------------------------------------------- */
56 const unsigned int vpAfma4::njoint = 4;
57 
63 vpAfma4::vpAfma4() : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
64 {
65  // Set the default parameters in case of the config files are not available.
66 
67  //
68  // Geometric model constant parameters
69  //
70  this->_a1 = 0.205; // distance along x2
71  this->_d3 = 0.403; // distance along z2
72  this->_d4 = 0.14; // distance along z3
73 
74  // Maximal value of the joints
75  this->_joint_max[0] = 1.8; // rad
76  this->_joint_max[1] = 0.9; // meter
77  this->_joint_max[2] = 0.9; // rad
78  this->_joint_max[3] = 0.76; // rad
79  // Minimal value of the joints
80  this->_joint_min[0] = -1.5; // rad
81  this->_joint_min[1] = -0.9; // meter
82  this->_joint_min[2] = -3.5; // rad
83  this->_joint_min[3] = -0.76; // rad
84 
85  // Camera extrinsic parameters: effector to camera frame
86  this->_etc[0] = 0.; // Translation
87  this->_etc[1] = 0.;
88  this->_etc[2] = 0.;
89  this->_erc[0] = 0.; // Rotation
90  this->_erc[1] = -M_PI / 2.;
91  this->_erc[2] = 0;
92 
94  this->_eMc.buildFrom(_etc, eRc);
95 
96  init();
97 }
98 
103 void vpAfma4::init(void) { return; }
104 
137 {
139  fMc = get_fMc(q);
140 
141  return fMc;
142 }
143 
174 {
176  get_fMc(q, fMc);
177 
178  return fMc;
179 }
180 
208 {
209 
210  // Compute the direct geometric model: fMe = transformation between
211  // fix and end effector frame.
213 
214  get_fMe(q, fMe);
215 
216  fMc = fMe * this->_eMc;
217 
218  return;
219 }
220 
257 {
258  double q1 = q[0]; // rot touret
259  double q2 = q[1]; // vertical translation
260  double q4 = q[2]; // pan
261  double q5 = q[3]; // tilt
262 
263  double c1 = cos(q1);
264  double s1 = sin(q1);
265  double c4 = cos(q4);
266  double s4 = sin(q4);
267  double c5 = cos(q5);
268  double s5 = sin(q5);
269 
270  /* Calcul du modele d'apres les angles. */
271  fMe[0][0] = c1 * s4 * c5 + s1 * c4 * c5;
272  fMe[0][1] = -c1 * s4 * s5 - s1 * c4 * s5;
273  fMe[0][2] = c1 * c4 - s1 * s4;
274  fMe[0][3] = c1 * this->_a1 - s1 * (this->_d3);
275 
276  fMe[1][0] = s1 * s4 * c5 - c1 * c4 * c5;
277  fMe[1][1] = -s1 * s4 * s5 + c1 * c4 * s5;
278  fMe[1][2] = s1 * c4 + c1 * s4;
279  fMe[1][3] = s1 * this->_a1 + c1 * (this->_d3);
280 
281  fMe[2][0] = -s5;
282  fMe[2][1] = -c5;
283  fMe[2][2] = 0.f;
284  fMe[2][3] = this->_d4 + q2;
285 
286  fMe[3][0] = 0.f;
287  fMe[3][1] = 0.f;
288  fMe[3][2] = 0.f;
289  fMe[3][3] = 1;
290 
291  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
292 
293  return;
294 }
295 
306 void vpAfma4::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
307 
318 {
320  get_cMe(cMe);
321 
322  cVe.buildFrom(cMe);
323 
324  return;
325 }
326 
346 {
347  vpHomogeneousMatrix fMc, cMf;
348  get_fMc(q, fMc);
349  cMf = fMc.inverse();
350 
351  cVf.buildFrom(cMf);
352 
353  return;
354 }
355 
392 void vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
393 {
394  double q4 = q[2]; // pan
395  double q5 = q[3]; // tilt
396 
397  double c4 = cos(q4);
398  double s4 = sin(q4);
399  double c5 = cos(q5);
400  double s5 = sin(q5);
401 
402  eJe.resize(6, 4);
403 
404  eJe = 0;
405 
406  eJe[0][0] = -(this->_a1 * c4 + this->_d3 * s4) * c5;
407  eJe[0][1] = -s5;
408  eJe[1][0] = (this->_a1 * c4 + this->_d3 * s4) * s5;
409  eJe[1][1] = -c5;
410  eJe[2][0] = (this->_a1 * s4 - this->_d3 * c4);
411  eJe[3][0] = eJe[3][2] = -s5;
412  eJe[4][0] = eJe[4][2] = -c5;
413  eJe[5][3] = 1.;
414 }
415 
447 void vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
448 {
449  fJe.resize(6, 4);
450 
451  double q1 = q[0]; // rot touret
452  double q4 = q[2]; // pan
453 
454  double c1 = cos(q1);
455  double s1 = sin(q1);
456  double c14 = cos(q1 + q4);
457  double s14 = sin(q1 + q4);
458 
459  fJe = 0;
460 
461  fJe[0][0] = -s1 * this->_a1 - c1 * this->_d3;
462 
463  fJe[1][0] = c1 * this->_a1 - s1 * this->_d3;
464 
465  fJe[2][1] = 1.0;
466 
467  fJe[3][3] = c14;
468 
469  fJe[4][3] = s14;
470 
471  fJe[5][0] = fJe[5][2] = 1.0;
472 }
473 
503 void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
504 {
505  fJe_inverse.resize(4, 6);
506  fJe_inverse = 0;
507 
508  double q1 = q[0]; // rot touret
509  double q4 = q[2]; // pan
510 
511  double c1 = cos(q1);
512  double s1 = sin(q1);
513  double c14 = cos(q1 + q4);
514  double s14 = sin(q1 + q4);
515 
516  double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;
517 
518  fJe_inverse[0][0] = (-s1 * this->_a1 - c1 * this->_d3) / det;
519  fJe_inverse[0][1] = (c1 * this->_a1 - s1 * this->_d3) / det;
520 
521  fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;
522 
523  fJe_inverse[2][0] = -fJe_inverse[0][0];
524  fJe_inverse[2][1] = -fJe_inverse[0][1];
525 
526  fJe_inverse[3][3] = c14;
527  fJe_inverse[3][4] = s14;
528 }
529 
539 {
540  vpColVector qmin(4);
541  for (unsigned int i = 0; i < 4; i++)
542  qmin[i] = this->_joint_min[i];
543  return qmin;
544 }
545 
555 {
556  vpColVector qmax(4);
557  for (unsigned int i = 0; i < 4; i++)
558  qmax[i] = this->_joint_max[i];
559  return qmax;
560 }
561 
571 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4)
572 {
573  vpRotationMatrix eRc;
574  afma4._eMc.extract(eRc);
575  vpRxyzVector rxyz(eRc);
576 
577  os << "Joint Max:" << std::endl
578  << "\t" << afma4._joint_max[0] << "\t" << afma4._joint_max[1] << "\t" << afma4._joint_max[2] << "\t"
579  << afma4._joint_max[3] << "\t" << std::endl
580 
581  << "Joint Min: " << std::endl
582  << "\t" << afma4._joint_min[0] << "\t" << afma4._joint_min[1] << "\t" << afma4._joint_min[2] << "\t"
583  << afma4._joint_min[3] << "\t" << std::endl
584 
585  << "a1: " << std::endl
586  << "\t" << afma4._a1 << "\t" << std::endl
587 
588  << "d3: " << std::endl
589  << "\t" << afma4._d3 << "\t" << std::endl
590 
591  << "d4: " << std::endl
592  << "\t" << afma4._d4 << "\t" << std::endl
593 
594  << "eMc: " << std::endl
595  << "\tTranslation (m): " << afma4._eMc[0][3] << " " << afma4._eMc[1][3] << " " << afma4._eMc[2][3] << "\t"
596  << std::endl
597  << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
598  << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
599  << "\t" << std::endl;
600 
601  return os;
602 }
Modelization of Irisa's cylindrical robot named Afma4.
Definition: vpAfma4.h:108
double _d3
Definition: vpAfma4.h:142
vpColVector getJointMin() const
Definition: vpAfma4.cpp:538
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma4.cpp:447
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma4.cpp:173
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma4.cpp:306
vpTranslationVector _etc
Definition: vpAfma4.h:148
static const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:137
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma4.cpp:317
vpRxyzVector _erc
Definition: vpAfma4.h:149
double _a1
Definition: vpAfma4.h:141
void init(void)
Definition: vpAfma4.cpp:103
double _d4
Definition: vpAfma4.h:143
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma4.cpp:392
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition: vpAfma4.cpp:503
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition: vpAfma4.cpp:345
vpColVector getJointMax() const
Definition: vpAfma4.cpp:554
vpHomogeneousMatrix _eMc
Definition: vpAfma4.h:151
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma4.cpp:256
double _joint_min[4]
Definition: vpAfma4.h:145
double _joint_max[4]
Definition: vpAfma4.h:144
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma4.cpp:136
vpAfma4()
Definition: vpAfma4.cpp:63
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:352
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:600
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
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: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.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)