Visual Servoing Platform  version 3.0.0
vpPtu46.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Interface for the ptu-46 robot.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
38 
39 
40 /* ----------------------------------------------------------------------- */
41 /* --- INCLUDE ----------------------------------------------------------- */
42 /* ----------------------------------------------------------------------- */
43 
44 
45 #include <visp3/robot/vpPtu46.h>
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/robot/vpRobotException.h>
48 
49 
50 /* Inclusion des fichiers standards. */
51 #include <math.h>
52 #include <visp3/core/vpMath.h>
53 
54 /* ------------------------------------------------------------------------ */
55 /* --- COMPUTE ------------------------------------------------------------ */
56 /* ------------------------------------------------------------------------ */
57 const unsigned int vpPtu46::ndof = 2; /*<! Pan and tilt are considered. */
58 const float vpPtu46::L = 0.0765f;
60 const float vpPtu46::h = 0.068f; /*<! Vertical offset from last joint to
61  camera frame. */
62 
63 
74 void
76 {
77  if (q.getRows() != 2) {
78  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
79  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
80  }
81 
82  double q1 = q[0]; // pan
83  double q2 = q[1]; // tilt
84 
85  double c1 = cos(q1);
86  double s1 = sin(q1);
87  double c2 = cos(q2);
88  double s2 = sin(q2);
89 
90  fMc[0][0] = s1;
91  fMc[0][1] = c1*s2;
92  fMc[0][2] = c1*c2;
93  fMc[0][3] = -h*c1*s2 - L*s1;
94 
95  fMc[1][0] = -c1;
96  fMc[1][1] = s1*s2;
97  fMc[1][2] = s1*c2;
98  fMc[1][3] = -h*s1*s2 + L*c1;
99 
100  fMc[2][0] = 0;
101  fMc[2][1] = -c2;
102  fMc[2][2] = s2;
103  fMc[2][3] = h*c2;
104 
105  fMc[3][0] = 0;
106  fMc[3][1] = 0;
107  fMc[3][2] = 0;
108  fMc[3][3] = 1;
109 
110  vpCDEBUG (6) << "Position de la camera: " << std::endl << fMc;
111 
112  return ;
113 }
114 
127 {
129 
130  computeMGD (q, fMc);
131 
132  return fMc;
133 }
134 
144 void
146 {
148 
149  computeMGD (q, fMc);
150  r.buildFrom(fMc.inverse());
151 
152  return ;
153 }
154 
155 
156 
157 /* ---------------------------------------------------------------------- */
158 /* --- CONSTRUCTOR ------------------------------------------------------ */
159 /* ---------------------------------------------------------------------- */
160 
167 {
168  init();
169 }
170 /* ---------------------------------------------------------------------- */
171 /* --- PRIVATE ---------------------------------------------------------- */
172 /* ---------------------------------------------------------------------- */
173 
174 
179 void
181 {
182  return ;
183 }
184 
185 
186 /* ----------------------------------------------------------------------- */
187 /* --- DISPLAY ----------------------------------------------------------- */
188 /* ----------------------------------------------------------------------- */
189 
190 VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpPtu46 & /* constant */)
191 {
192  os
193  << "Geometric parameters: " << std::endl
194  << "L: "
195  << "\t" << vpPtu46::L << std::endl
196  << "h: "
197  << "\t" << vpPtu46::h << std::endl;
198 
199  return os;
200 }
201 
202 
213 void
215 {
216  vpHomogeneousMatrix cMe ;
217  get_cMe(cMe) ;
218 
219  cVe.buildFrom(cMe) ;
220 }
221 
231 void
233 {
234  vpHomogeneousMatrix eMc ;
235 
236  eMc[0][0] = 0;
237  eMc[0][1] = -1;
238  eMc[0][2] = 0;
239  eMc[0][3] = h;
240 
241  eMc[1][0] = 1;
242  eMc[1][1] = 0;
243  eMc[1][2] = 0;
244  eMc[1][3] = -L;
245 
246  eMc[2][0] = 0;
247  eMc[2][1] = 0;
248  eMc[2][2] = 1;
249  eMc[2][3] = 0;
250 
251  eMc[3][0] = 0;
252  eMc[3][1] = 0;
253  eMc[3][2] = 0;
254  eMc[3][3] = 1;
255 
256  cMe = eMc.inverse() ;
257 }
258 
271 void
272 vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
273 {
274 
275 
276  eJe.resize(6,2) ;
277 
278  if (q.getRows() != 2) {
279  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
280  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
281  }
282 
283  double s2 = sin(q[1]) ;
284  double c2 = cos(q[1]) ;
285 
286  eJe = 0;
287 
288  eJe[3][0] = c2;
289  eJe[4][1] = 1;
290  eJe[5][0] = s2;
291 }
292 
302 void
303 vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe) const
304 {
305 
306  if (q.getRows() != 2) {
307  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
308  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
309  }
310 
311  fJe.resize(6,2) ;
312 
313  double s1 = sin(q[0]) ;
314  double c1 = cos(q[0]) ;
315 
316  fJe = 0;
317 
318  fJe[3][1] = s1;
319  fJe[4][1] = -c1;
320  fJe[5][0] = 1;
321 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpPtu46.cpp:303
static const float L
Definition: vpPtu46.h:85
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
Implementation of an homogeneous matrix and operations on such kind of matrices.
#define vpERROR_TRACE
Definition: vpDebug.h:391
error that can be emited by ViSP classes.
Definition: vpException.h:73
void get_cMe(vpHomogeneousMatrix &_cMe) const
Definition: vpPtu46.cpp:232
static const unsigned int ndof
Definition: vpPtu46.h:82
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static const float h
Definition: vpPtu46.h:86
#define vpCDEBUG(level)
Definition: vpDebug.h:502
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:267
Implementation of a velocity twist matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpPtu46.cpp:75
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Definition: vpPtu46.cpp:214
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpPtu46.cpp:272
Jacobian, geometric model functionnalities... for ptu46, pan, tilt head from Directed Perception...
Definition: vpPtu46.h:76
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
vpHomogeneousMatrix inverse() const
vpPtu46(void)
Definition: vpPtu46.cpp:166
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
void init(void)
Definition: vpPtu46.cpp:180