ViSP  2.10.0
vpAfma4.cpp
1 /****************************************************************************
2  *
3  * $Id: vpAfma4.cpp 4632 2014-02-03 17:06:40Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface for the Irisa's Afma4 robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
50 #include <visp/vpDebug.h>
51 #include <visp/vpVelocityTwistMatrix.h>
52 #include <visp/vpRobotException.h>
53 #include <visp/vpCameraParameters.h>
54 #include <visp/vpRxyzVector.h>
55 #include <visp/vpTranslationVector.h>
56 #include <visp/vpRotationMatrix.h>
57 #include <visp/vpAfma4.h>
58 
59 
60 /* ----------------------------------------------------------------------- */
61 /* --- STATIC ------------------------------------------------------------ */
62 /* ---------------------------------------------------------------------- */
63 const unsigned int vpAfma4::njoint = 4;
64 
65 
72  : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
73 {
74  // Set the default parameters in case of the config files on the NAS
75  // at Inria are not available.
76 
77  //
78  // Geometric model constant parameters
79  //
80  this->_a1 = 0.205; // distance along x2
81  this->_d3 = 0.403; // distance along z2
82  this->_d4 = 0.14; // distance along z3
83 
84  // Maximal value of the joints
85  this->_joint_max[0] = 1.8; // rad
86  this->_joint_max[1] = 0.9; // meter
87  this->_joint_max[2] = 0.9; // rad
88  this->_joint_max[3] = 0.76; // rad
89  // Minimal value of the joints
90  this->_joint_min[0] = -1.5; // rad
91  this->_joint_min[1] = -0.9; // meter
92  this->_joint_min[2] = -3.5; // rad
93  this->_joint_min[3] = -0.76;// rad
94 
95  // Camera extrinsic parameters: effector to camera frame
96  this->_etc[0] = 0.; // Translation
97  this->_etc[1] = 0.;
98  this->_etc[2] = 0.;
99  this->_erc[0] = 0.; // Rotation
100  this->_erc[1] = -M_PI/2.;
101  this->_erc[2] = 0;
102 
103  vpRotationMatrix eRc(_erc);
104  this->_eMc.buildFrom(_etc, eRc);
105 
106  init();
107 }
108 
109 
114 void
116 {
117  return;
118 }
119 
153 {
155  fMc = get_fMc(q);
156 
157  return fMc;
158 }
159 
190 vpAfma4::get_fMc (const vpColVector & q) const
191 {
193  get_fMc(q, fMc);
194 
195  return fMc;
196 }
197 
224 void
226 {
227 
228  // Compute the direct geometric model: fMe = transformation between
229  // fix and end effector frame.
231 
232  get_fMe(q, fMe);
233 
234  fMc = fMe * this->_eMc;
235 
236  return;
237 }
238 
272 void
274 {
275  double q1 = q[0]; // rot touret
276  double q2 = q[1]; // vertical translation
277  double q4 = q[2]; // pan
278  double q5 = q[3]; // tilt
279 
280  double c1 = cos(q1);
281  double s1 = sin(q1);
282  double c4 = cos(q4);
283  double s4 = sin(q4);
284  double c5 = cos(q5);
285  double s5 = sin(q5);
286 
287  /* Calcul du modele d'apres les angles. */
288  fMe[0][0] = c1*s4*c5 + s1*c4*c5;
289  fMe[0][1] = -c1*s4*s5 - s1*c4*s5;
290  fMe[0][2] = c1*c4 - s1*s4;
291  fMe[0][3] = c1*this->_a1 - s1*(this->_d3);
292 
293  fMe[1][0] = s1*s4*c5 - c1*c4*c5;
294  fMe[1][1] = -s1*s4*s5 + c1*c4*s5;
295  fMe[1][2] = s1*c4+c1*s4;
296  fMe[1][3] = s1*this->_a1 + c1*(this->_d3);
297 
298  fMe[2][0] = -s5;
299  fMe[2][1] = -c5;
300  fMe[2][2] = 0.f;
301  fMe[2][3] = this->_d4 + q2;
302 
303  fMe[3][0] = 0.f;
304  fMe[3][1] = 0.f;
305  fMe[3][2] = 0.f;
306  fMe[3][3] = 1;
307 
308  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
309 
310  return;
311 }
312 
323 void
325 {
326  cMe = this->_eMc.inverse();
327 }
328 
338 void
340 {
341  vpHomogeneousMatrix cMe ;
342  get_cMe(cMe) ;
343 
344  cVe.buildFrom(cMe) ;
345 
346  return;
347 }
348 
367 void
369 {
370  vpHomogeneousMatrix fMc, cMf ;
371  get_fMc(q, fMc) ;
372  cMf = fMc.inverse();
373 
374  cVf.buildFrom(cMf) ;
375 
376  return;
377 }
378 
415 void
416 vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
417 {
418  double q4 = q[2]; // pan
419  double q5 = q[3]; // tilt
420 
421  double c4 = cos(q4);
422  double s4 = sin(q4);
423  double c5 = cos(q5);
424  double s5 = sin(q5);
425 
426  eJe.resize(6, 4);
427 
428  eJe = 0;
429 
430  eJe[0][0] = -(this->_a1*c4 + this->_d3*s4)*c5; eJe[0][1] = -s5;
431  eJe[1][0] = (this->_a1*c4 + this->_d3*s4)*s5; eJe[1][1] = -c5;
432  eJe[2][0] = (this->_a1*s4 - this->_d3*c4);
433  eJe[3][0] = eJe[3][2] = -s5;
434  eJe[4][0] = eJe[4][2] = -c5;
435  eJe[5][3] = 1.;
436 }
437 
469 void
470 vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
471 {
472  fJe.resize(6,4) ;
473 
474  double q1 = q[0]; // rot touret
475  double q4 = q[2]; // pan
476 
477  double c1 = cos(q1);
478  double s1 = sin(q1);
479  double c14 = cos(q1 + q4);
480  double s14 = sin(q1 + q4);
481 
482  fJe = 0;
483 
484  fJe[0][0] = -s1*this->_a1 - c1*this->_d3;
485 
486  fJe[1][0] = c1*this->_a1 - s1*this->_d3;
487 
488  fJe[2][1] = 1.0;
489 
490  fJe[3][3] = c14;
491 
492  fJe[4][3] = s14;
493 
494  fJe[5][0] = fJe[5][2] = 1.0;
495 }
496 
526 void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
527 {
528  fJe_inverse.resize(4, 6) ;
529  fJe_inverse = 0;
530 
531  double q1 = q[0]; // rot touret
532  double q4 = q[2]; // pan
533 
534  double c1 = cos(q1);
535  double s1 = sin(q1);
536  double c14 = cos(q1 + q4);
537  double s14 = sin(q1 + q4);
538 
539  double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;
540 
541  fJe_inverse[0][0] = (-s1*this->_a1 - c1*this->_d3)/det;
542  fJe_inverse[0][1] = (c1*this->_a1 - s1*this->_d3)/det;
543 
544  fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;
545 
546  fJe_inverse[2][0] = - fJe_inverse[0][0];
547  fJe_inverse[2][1] = - fJe_inverse[0][1];
548 
549  fJe_inverse[3][3] = c14;
550  fJe_inverse[3][4] = s14;
551 }
552 
563 {
564  vpColVector qmin(4);
565  for (unsigned int i=0; i < 4; i ++)
566  qmin[i] = this->_joint_min[i];
567  return qmin;
568 }
569 
580 {
581  vpColVector qmax(4);
582  for (unsigned int i=0; i < 4; i ++)
583  qmax[i] = this->_joint_max[i];
584  return qmax;
585 }
586 
587 
588 
598 VISP_EXPORT std::ostream & operator << (std::ostream & os,
599  const vpAfma4 & afma4)
600 {
601  vpRotationMatrix eRc;
602  afma4._eMc.extract(eRc);
603  vpRxyzVector rxyz(eRc);
604 
605  os
606  << "Joint Max:" << std::endl
607  << "\t" << afma4._joint_max[0]
608  << "\t" << afma4._joint_max[1]
609  << "\t" << afma4._joint_max[2]
610  << "\t" << afma4._joint_max[3]
611  << "\t" << std::endl
612 
613  << "Joint Min: " << std::endl
614  << "\t" << afma4._joint_min[0]
615  << "\t" << afma4._joint_min[1]
616  << "\t" << afma4._joint_min[2]
617  << "\t" << afma4._joint_min[3]
618  << "\t" << std::endl
619 
620  << "a1: " << std::endl
621  << "\t" << afma4._a1
622  << "\t" << std::endl
623 
624  << "d3: " << std::endl
625  << "\t" << afma4._d3
626  << "\t" << std::endl
627 
628  << "d4: " << std::endl
629  << "\t" << afma4._d4
630  << "\t" << std::endl
631 
632  << "eMc: "<< std::endl
633  << "\tTranslation (m): "
634  << afma4._eMc[0][3] << " "
635  << afma4._eMc[1][3] << " "
636  << afma4._eMc[2][3]
637  << "\t" << std::endl
638  << "\tRotation Rxyz (rad) : "
639  << rxyz[0] << " "
640  << rxyz[1] << " "
641  << rxyz[2]
642  << "\t" << std::endl
643  << "\tRotation Rxyz (deg) : "
644  << vpMath::deg(rxyz[0]) << " "
645  << vpMath::deg(rxyz[1]) << " "
646  << vpMath::deg(rxyz[2])
647  << "\t" << std::endl;
648 
649  return os;
650 }
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
double _a1
Definition: vpAfma4.h:146
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma4.cpp:190
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma4.cpp:273
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma4.cpp:324
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
vpHomogeneousMatrix _eMc
Definition: vpAfma4.h:156
vpColVector getJointMax() const
Definition: vpAfma4.cpp:579
double _d4
Definition: vpAfma4.h:148
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma4.cpp:339
void init(void)
Definition: vpAfma4.cpp:115
vpAfma4()
Definition: vpAfma4.cpp:71
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition: vpAfma4.cpp:368
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma4.cpp:416
The vpRotationMatrix considers the particular case of a rotation matrix.
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
static const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:142
Modelisation of Irisa's cylindrical robot named Afma4.
Definition: vpAfma4.h:112
void extract(vpRotationMatrix &R) const
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma4.cpp:152
vpRxyzVector _erc
Definition: vpAfma4.h:154
static double deg(double rad)
Definition: vpMath.h:93
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpColVector getJointMin() const
Definition: vpAfma4.cpp:562
vpHomogeneousMatrix inverse() const
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition: vpAfma4.cpp:526
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
double _d3
Definition: vpAfma4.h:147
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma4.cpp:470