Visual Servoing Platform  version 3.2.1 under development (2019-05-20)
vpBiclops.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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 Biclops robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 /* ----------------------------------------------------------------------- */
40 /* --- INCLUDE ----------------------------------------------------------- */
41 /* ----------------------------------------------------------------------- */
42 
43 #include <math.h>
44 #include <visp3/core/vpDebug.h>
45 #include <visp3/core/vpMath.h>
46 #include <visp3/robot/vpBiclops.h>
47 #include <visp3/robot/vpRobotException.h>
48 
49 /* ------------------------------------------------------------------------ */
50 /* --- COMPUTE ------------------------------------------------------------ */
51 /* ------------------------------------------------------------------------ */
52 const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
53 const float vpBiclops::h = 0.048f; /*<! Vertical offset from last joint to camera frame. */
54 const float vpBiclops::panJointLimit = (float)(M_PI);
56 const float vpBiclops::tiltJointLimit = (float)(M_PI / 4.5);
59 const float vpBiclops::speedLimit = (float)(M_PI / 3.0);
76 {
78  fMc = fMe * cMe_.inverse();
79 
80  vpCDEBUG(6) << "camera position: " << std::endl << fMc;
81 
82  return;
83 }
84 
96 {
98  fMc = fMe * cMe_.inverse();
99 
100  vpCDEBUG(6) << "camera position: " << std::endl << fMc;
101 
102  return;
103 }
104 
120 {
122 
123  computeMGD(q, fMc);
124 
125  return fMc;
126 }
127 
139 {
141 
142  get_fMc(q, fMc);
143 
144  return fMc;
145 }
146 
158 {
160 
161  if (q.getRows() != 2) {
162  vpERROR_TRACE("Bad dimension for biclops articular vector");
163  throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
164  }
165 
166  double q1 = q[0]; // pan
167  double q2 = q[1]; // tilt
168 
169  double c1 = cos(q1);
170  double s1 = sin(q1);
171  double c2 = cos(q2);
172  double s2 = sin(q2);
173 
174  if (dh_model_ == DH1) {
175  fMe[0][0] = -c1 * s2;
176  fMe[0][1] = -s1;
177  fMe[0][2] = c1 * c2;
178  fMe[0][3] = 0;
179 
180  fMe[1][0] = -s1 * s2;
181  fMe[1][1] = c1;
182  fMe[1][2] = s1 * c2;
183  fMe[1][3] = 0;
184 
185  fMe[2][0] = -c2;
186  fMe[2][1] = 0;
187  fMe[2][2] = -s2;
188  fMe[2][3] = 0;
189 
190  fMe[3][0] = 0;
191  fMe[3][1] = 0;
192  fMe[3][2] = 0;
193  fMe[3][3] = 1;
194  } else {
195  fMe[0][0] = c1 * s2;
196  fMe[0][1] = -s1;
197  fMe[0][2] = c1 * c2;
198  fMe[0][3] = 0;
199 
200  fMe[1][0] = s1 * s2;
201  fMe[1][1] = c1;
202  fMe[1][2] = s1 * c2;
203  fMe[1][3] = 0;
204 
205  fMe[2][0] = -c2;
206  fMe[2][1] = 0;
207  fMe[2][2] = s2;
208  fMe[2][3] = 0;
209 
210  fMe[3][0] = 0;
211  fMe[3][1] = 0;
212  fMe[3][2] = 0;
213  fMe[3][3] = 1;
214  }
215 
216  return fMe;
217 }
218 
234 {
236 
237  get_fMc(q, fMc);
238  fvc.buildFrom(fMc.inverse());
239 
240  return;
241 }
242 
253 void vpBiclops::get_fMc(const vpColVector &q, vpPoseVector &fvc) const
254 {
256 
257  get_fMc(q, fMc);
258  fvc.buildFrom(fMc.inverse());
259 
260  return;
261 }
262 
263 /* ---------------------------------------------------------------------- */
264 /* --- CONSTRUCTOR ------------------------------------------------------ */
265 /* ---------------------------------------------------------------------- */
266 
273 /* ---------------------------------------------------------------------- */
274 /* --- PRIVATE ---------------------------------------------------------- */
275 /* ---------------------------------------------------------------------- */
276 
283 {
284  dh_model_ = DH1;
285  set_cMe();
286  return;
287 }
288 
289 /* ----------------------------------------------------------------------- */
290 /* --- DISPLAY ----------------------------------------------------------- */
291 /* ----------------------------------------------------------------------- */
292 
293 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops & /*constant*/)
294 {
295  os << "Geometric parameters: " << std::endl
296  << "h: "
297  << "\t" << vpBiclops::h << std::endl;
298 
299  return os;
300 }
301 
313 
322 {
324 
325  eMc[0][0] = 0;
326  eMc[0][1] = -1;
327  eMc[0][2] = 0;
328  eMc[0][3] = h;
329 
330  eMc[1][0] = 1;
331  eMc[1][1] = 0;
332  eMc[1][2] = 0;
333  eMc[1][3] = 0;
334 
335  eMc[2][0] = 0;
336  eMc[2][1] = 0;
337  eMc[2][2] = 1;
338  eMc[2][3] = 0;
339 
340  eMc[3][0] = 0;
341  eMc[3][1] = 0;
342  eMc[3][2] = 0;
343  eMc[3][3] = 1;
344 
345  cMe_ = eMc.inverse();
346 }
347 
360 void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const
361 {
362  eJe.resize(6, 2);
363 
364  if (q.getRows() != 2) {
365  vpERROR_TRACE("Bad dimension for biclops articular vector");
366  throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
367  }
368 
369  double s2 = sin(q[1]);
370  double c2 = cos(q[1]);
371 
372  eJe = 0;
373 
374  if (dh_model_ == DH1) {
375  eJe[3][0] = -c2;
376  eJe[4][1] = 1;
377  eJe[5][0] = -s2;
378  } else {
379  eJe[3][0] = -c2;
380  eJe[4][1] = -1;
381  eJe[5][0] = s2;
382  }
383 }
394 void vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const
395 {
396  if (q.getRows() != 2) {
397  vpERROR_TRACE("Bad dimension for biclops articular vector");
398  throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
399  }
400 
401  fJe.resize(6, 2);
402 
403  double s1 = sin(q[0]);
404  double c1 = cos(q[0]);
405 
406  fJe = 0;
407 
408  if (dh_model_ == DH1) {
409  fJe[3][1] = -s1;
410  fJe[4][1] = c1;
411  fJe[5][0] = 1;
412  } else {
413  fJe[3][1] = s1;
414  fJe[4][1] = -c1;
415  fJe[5][0] = 1;
416  }
417 }
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
Definition: vpBiclops.h:77
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpHomogeneousMatrix cMe_
Definition: vpBiclops.h:137
static const unsigned int ndof
Definition: vpBiclops.h:126
Implementation of an homogeneous matrix and operations on such kind of matrices.
static const float tiltJointLimit
Definition: vpBiclops.h:132
static const float speedLimit
Definition: vpBiclops.h:133
#define vpERROR_TRACE
Definition: vpDebug.h:393
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Definition: vpBiclops.cpp:312
error that can be emited by ViSP classes.
Definition: vpException.h:71
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:95
static const float panJointLimit
Definition: vpBiclops.h:131
static const float h
Definition: vpBiclops.h:129
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
DenavitHartenbergModel dh_model_
Definition: vpBiclops.h:136
#define vpCDEBUG(level)
Definition: vpDebug.h:511
unsigned int getRows() const
Definition: vpArray2D.h:289
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:394
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:75
void set_cMe()
Definition: vpBiclops.cpp:321
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:360
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpBiclops &constant)
Definition: vpBiclops.cpp:293
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix get_fMe(const vpColVector &q) const
Definition: vpBiclops.cpp:157
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:282
vpBiclops(void)
Definition: vpBiclops.cpp:272