Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpBiclops.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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 Biclops robot.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
38 /* ----------------------------------------------------------------------- */
39 /* --- INCLUDE ----------------------------------------------------------- */
40 /* ----------------------------------------------------------------------- */
41 
42 
43 
44 #include <visp3/robot/vpBiclops.h>
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/robot/vpRobotException.h>
47 #include <visp3/core/vpMath.h>
48 #include <math.h>
49 
50 
51 /* ------------------------------------------------------------------------ */
52 /* --- COMPUTE ------------------------------------------------------------ */
53 /* ------------------------------------------------------------------------ */
54 const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
55 const float vpBiclops::h = 0.048f; /*<! Vertical offset from last joint to camera frame. */
56 const float vpBiclops::panJointLimit = (float)(M_PI);
57 const float vpBiclops::tiltJointLimit = (float)(M_PI/4.5);
59 const float vpBiclops::speedLimit = (float)(M_PI/3.0);
75 void
77 {
79  fMc = fMe * cMe_.inverse();
80 
81  vpCDEBUG (6) << "camera position: " << std::endl << fMc;
82 
83  return ;
84 }
85 
96 void
98 {
100  fMc = fMe * cMe_.inverse();
101 
102  vpCDEBUG (6) << "camera position: " << std::endl << fMc;
103 
104  return ;
105 }
106 
123 {
125 
126  computeMGD (q, fMc);
127 
128  return fMc;
129 }
130 
143 {
145 
146  get_fMc (q, fMc);
147 
148  return fMc;
149 }
150 
163 {
165 
166  if (q.getRows() != 2) {
167  vpERROR_TRACE("Bad dimension for biclops articular vector");
168  throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
169  }
170 
171  double q1 = q[0]; // pan
172  double q2 = q[1]; // tilt
173 
174  double c1 = cos(q1);
175  double s1 = sin(q1);
176  double c2 = cos(q2);
177  double s2 = sin(q2);
178 
179  if (dh_model_ == DH1)
180  {
181  fMe[0][0] = -c1*s2;
182  fMe[0][1] = -s1;
183  fMe[0][2] = c1*c2;
184  fMe[0][3] = 0;
185 
186  fMe[1][0] = -s1*s2;
187  fMe[1][1] = c1;
188  fMe[1][2] = s1*c2;
189  fMe[1][3] = 0;
190 
191  fMe[2][0] = -c2;
192  fMe[2][1] = 0;
193  fMe[2][2] = -s2;
194  fMe[2][3] = 0;
195 
196  fMe[3][0] = 0;
197  fMe[3][1] = 0;
198  fMe[3][2] = 0;
199  fMe[3][3] = 1;
200  }
201  else
202  {
203  fMe[0][0] = c1*s2;
204  fMe[0][1] = -s1;
205  fMe[0][2] = c1*c2;
206  fMe[0][3] = 0;
207 
208  fMe[1][0] = s1*s2;
209  fMe[1][1] = c1;
210  fMe[1][2] = s1*c2;
211  fMe[1][3] = 0;
212 
213  fMe[2][0] = -c2;
214  fMe[2][1] = 0;
215  fMe[2][2] = s2;
216  fMe[2][3] = 0;
217 
218  fMe[3][0] = 0;
219  fMe[3][1] = 0;
220  fMe[3][2] = 0;
221  fMe[3][3] = 1;
222  }
223 
224  return fMe;
225 }
226 
241 void
243 {
245 
246  get_fMc (q, fMc);
247  fvc.buildFrom(fMc.inverse());
248 
249  return ;
250 }
251 
262 void
264 {
266 
267  get_fMc (q, fMc);
268  fvc.buildFrom(fMc.inverse());
269 
270  return ;
271 }
272 
273 
274 /* ---------------------------------------------------------------------- */
275 /* --- CONSTRUCTOR ------------------------------------------------------ */
276 /* ---------------------------------------------------------------------- */
277 
284  : dh_model_(DH1), cMe_()
285 {
286  init();
287 }
288 /* ---------------------------------------------------------------------- */
289 /* --- PRIVATE ---------------------------------------------------------- */
290 /* ---------------------------------------------------------------------- */
291 
297 void
299 {
300  dh_model_ = DH1;
301  set_cMe();
302  return ;
303 }
304 
305 
306 /* ----------------------------------------------------------------------- */
307 /* --- DISPLAY ----------------------------------------------------------- */
308 /* ----------------------------------------------------------------------- */
309 
310 VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpBiclops & /*constant*/)
311 {
312  os
313  << "Geometric parameters: " << std::endl
314  << "h: "
315  << "\t" << vpBiclops::h << std::endl;
316 
317  return os;
318 }
319 
320 
331 void
333 {
334  cVe.buildFrom(cMe_) ;
335 }
336 
344 void
346 {
347  vpHomogeneousMatrix eMc ;
348 
349  eMc[0][0] = 0;
350  eMc[0][1] = -1;
351  eMc[0][2] = 0;
352  eMc[0][3] = h;
353 
354  eMc[1][0] = 1;
355  eMc[1][1] = 0;
356  eMc[1][2] = 0;
357  eMc[1][3] = 0;
358 
359  eMc[2][0] = 0;
360  eMc[2][1] = 0;
361  eMc[2][2] = 1;
362  eMc[2][3] = 0;
363 
364  eMc[3][0] = 0;
365  eMc[3][1] = 0;
366  eMc[3][2] = 0;
367  eMc[3][3] = 1;
368 
369  cMe_ = eMc.inverse() ;
370 }
371 
384 void
386 {
387  eJe.resize(6,2) ;
388 
389  if (q.getRows() != 2) {
390  vpERROR_TRACE("Bad dimension for biclops articular vector");
391  throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
392  }
393 
394  double s2 = sin(q[1]) ;
395  double c2 = cos(q[1]) ;
396 
397  eJe = 0;
398 
399  if (dh_model_ == DH1)
400  {
401  eJe[3][0] = -c2;
402  eJe[4][1] = 1;
403  eJe[5][0] = -s2;
404  }
405  else
406  {
407  eJe[3][0] = -c2;
408  eJe[4][1] = -1;
409  eJe[5][0] = s2;
410 
411  }
412 
413 }
424 void
426 {
427  if (q.getRows() != 2) {
428  vpERROR_TRACE("Bad dimension for biclops articular vector");
429  throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
430  }
431 
432  fJe.resize(6,2) ;
433 
434  double s1 = sin(q[0]) ;
435  double c1 = cos(q[0]) ;
436 
437  fJe = 0;
438 
439  if (dh_model_ == DH1)
440  {
441  fJe[3][1] = -s1;
442  fJe[4][1] = c1;
443  fJe[5][0] = 1;
444  }
445  else
446  {
447  fJe[3][1] = s1;
448  fJe[4][1] = -c1;
449  fJe[5][0] = 1;
450  }
451 }
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
Definition: vpBiclops.h:74
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
vpHomogeneousMatrix cMe_
Definition: vpBiclops.h:135
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
static const unsigned int ndof
Definition: vpBiclops.h:123
Implementation of an homogeneous matrix and operations on such kind of matrices.
static const float tiltJointLimit
Definition: vpBiclops.h:129
static const float speedLimit
Definition: vpBiclops.h:130
#define vpERROR_TRACE
Definition: vpDebug.h:391
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Definition: vpBiclops.cpp:332
error that can be emited by ViSP classes.
Definition: vpException.h:73
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:97
static const float panJointLimit
Definition: vpBiclops.h:128
static const float h
Definition: vpBiclops.h:126
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
DenavitHartenbergModel dh_model_
Definition: vpBiclops.h:134
#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 get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:425
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:76
void set_cMe()
Definition: vpBiclops.cpp:345
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:385
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
vpHomogeneousMatrix get_fMe(const vpColVector &q) const
Definition: vpBiclops.cpp:162
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
void init(void)
Definition: vpBiclops.cpp:298
vpBiclops(void)
Definition: vpBiclops.cpp:283