ViSP  2.10.0
vpBiclops.cpp
1 /****************************************************************************
2  *
3  * $Id: vpBiclops.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 Biclops robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 /* ----------------------------------------------------------------------- */
43 /* --- INCLUDE ----------------------------------------------------------- */
44 /* ----------------------------------------------------------------------- */
45 
46 
47 
48 #include <visp/vpBiclops.h>
49 #include <visp/vpDebug.h>
50 #include <visp/vpRobotException.h>
51 #include <visp/vpMath.h>
52 #include <math.h>
53 
54 
55 /* ------------------------------------------------------------------------ */
56 /* --- COMPUTE ------------------------------------------------------------ */
57 /* ------------------------------------------------------------------------ */
58 const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
59 const float vpBiclops::h = 0.048f; /*<! Vertical offset from last joint to camera frame. */
60 const float vpBiclops::panJointLimit = (float)(M_PI);
61 const float vpBiclops::tiltJointLimit = (float)(M_PI/4.5);
63 const float vpBiclops::speedLimit = (float)(M_PI/3.0);
79 void
81 {
83  fMc = fMe * cMe_.inverse();
84 
85  vpCDEBUG (6) << "camera position: " << std::endl << fMc;
86 
87  return ;
88 }
89 
100 void
102 {
103  vpHomogeneousMatrix fMe = get_fMe(q);
104  fMc = fMe * cMe_.inverse();
105 
106  vpCDEBUG (6) << "camera position: " << std::endl << fMc;
107 
108  return ;
109 }
110 
127 {
129 
130  computeMGD (q, fMc);
131 
132  return fMc;
133 }
134 
147 {
149 
150  get_fMc (q, fMc);
151 
152  return fMc;
153 }
154 
167 {
169 
170  if (q.getRows() != 2) {
171  vpERROR_TRACE("Bad dimension for biclops articular vector");
172  throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
173  }
174 
175  double q1 = q[0]; // pan
176  double q2 = q[1]; // tilt
177 
178  double c1 = cos(q1);
179  double s1 = sin(q1);
180  double c2 = cos(q2);
181  double s2 = sin(q2);
182 
183  if (dh_model_ == DH1)
184  {
185  fMe[0][0] = -c1*s2;
186  fMe[0][1] = -s1;
187  fMe[0][2] = c1*c2;
188  fMe[0][3] = 0;
189 
190  fMe[1][0] = -s1*s2;
191  fMe[1][1] = c1;
192  fMe[1][2] = s1*c2;
193  fMe[1][3] = 0;
194 
195  fMe[2][0] = -c2;
196  fMe[2][1] = 0;
197  fMe[2][2] = -s2;
198  fMe[2][3] = 0;
199 
200  fMe[3][0] = 0;
201  fMe[3][1] = 0;
202  fMe[3][2] = 0;
203  fMe[3][3] = 1;
204  }
205  else
206  {
207  fMe[0][0] = c1*s2;
208  fMe[0][1] = -s1;
209  fMe[0][2] = c1*c2;
210  fMe[0][3] = 0;
211 
212  fMe[1][0] = s1*s2;
213  fMe[1][1] = c1;
214  fMe[1][2] = s1*c2;
215  fMe[1][3] = 0;
216 
217  fMe[2][0] = -c2;
218  fMe[2][1] = 0;
219  fMe[2][2] = s2;
220  fMe[2][3] = 0;
221 
222  fMe[3][0] = 0;
223  fMe[3][1] = 0;
224  fMe[3][2] = 0;
225  fMe[3][3] = 1;
226  }
227 
228  return fMe;
229 }
230 
245 void
247 {
249 
250  get_fMc (q, fMc);
251  fvc.buildFrom(fMc.inverse());
252 
253  return ;
254 }
255 
266 void
268 {
270 
271  get_fMc (q, fMc);
272  fvc.buildFrom(fMc.inverse());
273 
274  return ;
275 }
276 
277 
278 /* ---------------------------------------------------------------------- */
279 /* --- CONSTRUCTOR ------------------------------------------------------ */
280 /* ---------------------------------------------------------------------- */
281 
288  : dh_model_(DH1), cMe_()
289 {
290  init();
291 }
292 /* ---------------------------------------------------------------------- */
293 /* --- PRIVATE ---------------------------------------------------------- */
294 /* ---------------------------------------------------------------------- */
295 
301 void
303 {
304  dh_model_ = DH1;
305  set_cMe();
306  return ;
307 }
308 
309 
310 /* ----------------------------------------------------------------------- */
311 /* --- DISPLAY ----------------------------------------------------------- */
312 /* ----------------------------------------------------------------------- */
313 
314 VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpBiclops & /*constant*/)
315 {
316  os
317  << "Geometric parameters: " << std::endl
318  << "h: "
319  << "\t" << vpBiclops::h << std::endl;
320 
321  return os;
322 }
323 
324 
335 void
337 {
338  cVe.buildFrom(cMe_) ;
339 }
340 
348 void
350 {
351  vpHomogeneousMatrix eMc ;
352 
353  eMc[0][0] = 0;
354  eMc[0][1] = -1;
355  eMc[0][2] = 0;
356  eMc[0][3] = h;
357 
358  eMc[1][0] = 1;
359  eMc[1][1] = 0;
360  eMc[1][2] = 0;
361  eMc[1][3] = 0;
362 
363  eMc[2][0] = 0;
364  eMc[2][1] = 0;
365  eMc[2][2] = 1;
366  eMc[2][3] = 0;
367 
368  eMc[3][0] = 0;
369  eMc[3][1] = 0;
370  eMc[3][2] = 0;
371  eMc[3][3] = 1;
372 
373  cMe_ = eMc.inverse() ;
374 }
375 
388 void
390 {
391  eJe.resize(6,2) ;
392 
393  if (q.getRows() != 2) {
394  vpERROR_TRACE("Bad dimension for biclops articular vector");
395  throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
396  }
397 
398  double s2 = sin(q[1]) ;
399  double c2 = cos(q[1]) ;
400 
401  eJe = 0;
402 
403  if (dh_model_ == DH1)
404  {
405  eJe[3][0] = -c2;
406  eJe[4][1] = 1;
407  eJe[5][0] = -s2;
408  }
409  else
410  {
411  eJe[3][0] = -c2;
412  eJe[4][1] = -1;
413  eJe[5][0] = s2;
414 
415  }
416 
417 }
428 void
430 {
431  if (q.getRows() != 2) {
432  vpERROR_TRACE("Bad dimension for biclops articular vector");
433  throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
434  }
435 
436  fJe.resize(6,2) ;
437 
438  double s1 = sin(q[0]) ;
439  double c1 = cos(q[0]) ;
440 
441  fJe = 0;
442 
443  if (dh_model_ == DH1)
444  {
445  fJe[3][1] = -s1;
446  fJe[4][1] = c1;
447  fJe[5][0] = 1;
448  }
449  else
450  {
451  fJe[3][1] = s1;
452  fJe[4][1] = -c1;
453  fJe[5][0] = 1;
454  }
455 }
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
Definition: vpBiclops.h:78
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
vpHomogeneousMatrix cMe_
Definition: vpBiclops.h:139
static const unsigned int ndof
Definition: vpBiclops.h:127
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
static const float tiltJointLimit
Definition: vpBiclops.h:133
static const float speedLimit
Definition: vpBiclops.h:134
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Definition: vpBiclops.cpp:336
error that can be emited by ViSP classes.
Definition: vpException.h:76
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:101
vpPoseVector buildFrom(const vpHomogeneousMatrix &M)
static const float panJointLimit
Definition: vpBiclops.h:132
static const float h
Definition: vpBiclops.h:130
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
DenavitHartenbergModel dh_model_
Definition: vpBiclops.h:138
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
#define vpCDEBUG(level)
Definition: vpDebug.h:506
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:429
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:80
void set_cMe()
Definition: vpBiclops.cpp:349
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:389
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix get_fMe(const vpColVector &q) const
Definition: vpBiclops.cpp:166
void init(void)
Definition: vpBiclops.cpp:302
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
vpBiclops(void)
Definition: vpBiclops.cpp:287