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