Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpPtu46.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 ptu-46 robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 /* ----------------------------------------------------------------------- */
40 /* --- INCLUDE ----------------------------------------------------------- */
41 /* ----------------------------------------------------------------------- */
42 
43 #include <visp3/core/vpDebug.h>
44 #include <visp3/robot/vpPtu46.h>
45 #include <visp3/robot/vpRobotException.h>
46 
47 /* Inclusion des fichiers standards. */
48 #include <math.h>
49 #include <visp3/core/vpMath.h>
50 
51 /* ------------------------------------------------------------------------ */
52 /* --- COMPUTE ------------------------------------------------------------ */
53 /* ------------------------------------------------------------------------ */
54 const unsigned int vpPtu46::ndof = 2; /*<! Pan and tilt are considered. */
55 const float vpPtu46::L = 0.0765f;
57 const float vpPtu46::h = 0.068f; /*<! Vertical offset from last joint to
58  camera frame. */
59 
71 {
72  if (q.getRows() != 2) {
73  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
74  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
75  }
76 
77  double q1 = q[0]; // pan
78  double q2 = q[1]; // tilt
79 
80  double c1 = cos(q1);
81  double s1 = sin(q1);
82  double c2 = cos(q2);
83  double s2 = sin(q2);
84 
85  fMc[0][0] = s1;
86  fMc[0][1] = c1 * s2;
87  fMc[0][2] = c1 * c2;
88  fMc[0][3] = -h * c1 * s2 - L * s1;
89 
90  fMc[1][0] = -c1;
91  fMc[1][1] = s1 * s2;
92  fMc[1][2] = s1 * c2;
93  fMc[1][3] = -h * s1 * s2 + L * c1;
94 
95  fMc[2][0] = 0;
96  fMc[2][1] = -c2;
97  fMc[2][2] = s2;
98  fMc[2][3] = h * c2;
99 
100  fMc[3][0] = 0;
101  fMc[3][1] = 0;
102  fMc[3][2] = 0;
103  fMc[3][3] = 1;
104 
105  vpCDEBUG(6) << "Position de la camera: " << std::endl << fMc;
106 
107  return;
108 }
109 
121 {
123 
124  computeMGD(q, fMc);
125 
126  return fMc;
127 }
128 
139 {
141 
142  computeMGD(q, fMc);
143  r.buildFrom(fMc.inverse());
144 
145  return;
146 }
147 
148 /* ---------------------------------------------------------------------- */
149 /* --- CONSTRUCTOR ------------------------------------------------------ */
150 /* ---------------------------------------------------------------------- */
151 
157 vpPtu46::vpPtu46(void) { init(); }
158 /* ---------------------------------------------------------------------- */
159 /* --- PRIVATE ---------------------------------------------------------- */
160 /* ---------------------------------------------------------------------- */
161 
166 void vpPtu46::init() { return; }
167 
168 /* ----------------------------------------------------------------------- */
169 /* --- DISPLAY ----------------------------------------------------------- */
170 /* ----------------------------------------------------------------------- */
171 
172 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPtu46 & /* constant */)
173 {
174  os << "Geometric parameters: " << std::endl
175  << "L: "
176  << "\t" << vpPtu46::L << std::endl
177  << "h: "
178  << "\t" << vpPtu46::h << std::endl;
179 
180  return os;
181 }
182 
194 {
196  get_cMe(cMe);
197 
198  cVe.buildFrom(cMe);
199 }
200 
211 {
213 
214  eMc[0][0] = 0;
215  eMc[0][1] = -1;
216  eMc[0][2] = 0;
217  eMc[0][3] = h;
218 
219  eMc[1][0] = 1;
220  eMc[1][1] = 0;
221  eMc[1][2] = 0;
222  eMc[1][3] = -L;
223 
224  eMc[2][0] = 0;
225  eMc[2][1] = 0;
226  eMc[2][2] = 1;
227  eMc[2][3] = 0;
228 
229  eMc[3][0] = 0;
230  eMc[3][1] = 0;
231  eMc[3][2] = 0;
232  eMc[3][3] = 1;
233 
234  cMe = eMc.inverse();
235 }
236 
249 void vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
250 {
251 
252  eJe.resize(6, 2);
253 
254  if (q.getRows() != 2) {
255  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
256  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
257  }
258 
259  double s2 = sin(q[1]);
260  double c2 = cos(q[1]);
261 
262  eJe = 0;
263 
264  eJe[3][0] = c2;
265  eJe[4][1] = 1;
266  eJe[5][0] = s2;
267 }
268 
278 void vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe) const
279 {
280 
281  if (q.getRows() != 2) {
282  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
283  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
284  }
285 
286  fJe.resize(6, 2);
287 
288  double s1 = sin(q[0]);
289  double c1 = cos(q[0]);
290 
291  fJe = 0;
292 
293  fJe[3][1] = s1;
294  fJe[4][1] = -c1;
295  fJe[5][0] = 1;
296 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpPtu46 &constant)
Definition: vpPtu46.cpp:172
static const float L
Definition: vpPtu46.h:81
Implementation of an homogeneous matrix and operations on such kind of matrices.
void get_cMe(vpHomogeneousMatrix &_cMe) const
Definition: vpPtu46.cpp:210
#define vpERROR_TRACE
Definition: vpDebug.h:393
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
vpHomogeneousMatrix inverse() const
static const unsigned int ndof
Definition: vpPtu46.h:78
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Definition: vpPtu46.cpp:193
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpPtu46.cpp:70
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static const float h
Definition: vpPtu46.h:82
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
#define vpCDEBUG(level)
Definition: vpDebug.h:511
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpPtu46.cpp:249
Jacobian, geometric model functionnalities... for ptu46, pan, tilt head from Directed Perception...
Definition: vpPtu46.h:73
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
vpPtu46(void)
Definition: vpPtu46.cpp:157
void init(void)
Definition: vpPtu46.cpp:166
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpPtu46.cpp:278