Visual Servoing Platform  version 3.6.1 under development (2024-12-10)
vpPtu46.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 /* ----------------------------------------------------------------------- */
37 /* --- INCLUDE ----------------------------------------------------------- */
38 /* ----------------------------------------------------------------------- */
39 
40 #include <visp3/core/vpDebug.h>
41 #include <visp3/robot/vpPtu46.h>
42 #include <visp3/robot/vpRobotException.h>
43 
44 /* Inclusion des fichiers standards. */
45 #include <math.h>
46 #include <visp3/core/vpMath.h>
47 
48 BEGIN_VISP_NAMESPACE
49 /* ------------------------------------------------------------------------ */
50 /* --- COMPUTE ------------------------------------------------------------ */
51 /* ------------------------------------------------------------------------ */
52 const unsigned int vpPtu46::ndof = 2; /*<! Pan and tilt are considered. */
53 const float vpPtu46::L = 0.0765f;
55 const float vpPtu46::h = 0.068f; /*<! Vertical offset from last joint to
56  camera frame. */
57 
69 {
70  if (q.getRows() != 2) {
71  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
72  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
73  }
74 
75  double q1 = q[0]; // pan
76  double q2 = q[1]; // tilt
77 
78  double c1 = cos(q1);
79  double s1 = sin(q1);
80  double c2 = cos(q2);
81  double s2 = sin(q2);
82 
83  fMc[0][0] = s1;
84  fMc[0][1] = c1 * s2;
85  fMc[0][2] = c1 * c2;
86  fMc[0][3] = -h * c1 * s2 - L * s1;
87 
88  fMc[1][0] = -c1;
89  fMc[1][1] = s1 * s2;
90  fMc[1][2] = s1 * c2;
91  fMc[1][3] = -h * s1 * s2 + L * c1;
92 
93  fMc[2][0] = 0;
94  fMc[2][1] = -c2;
95  fMc[2][2] = s2;
96  fMc[2][3] = h * c2;
97 
98  fMc[3][0] = 0;
99  fMc[3][1] = 0;
100  fMc[3][2] = 0;
101  fMc[3][3] = 1;
102 
103  vpCDEBUG(6) << "Position de la camera: " << std::endl << fMc;
104 
105  return;
106 }
107 
119 {
121 
122  computeMGD(q, fMc);
123 
124  return fMc;
125 }
126 
137 {
139 
140  computeMGD(q, fMc);
141  r.buildFrom(fMc.inverse());
142 
143  return;
144 }
145 
146 /* ---------------------------------------------------------------------- */
147 /* --- CONSTRUCTOR ------------------------------------------------------ */
148 /* ---------------------------------------------------------------------- */
149 
155 vpPtu46::vpPtu46(void) { init(); }
156 /* ---------------------------------------------------------------------- */
157 /* --- PRIVATE ---------------------------------------------------------- */
158 /* ---------------------------------------------------------------------- */
159 
164 void vpPtu46::init() { return; }
165 
166 /* ----------------------------------------------------------------------- */
167 /* --- DISPLAY ----------------------------------------------------------- */
168 /* ----------------------------------------------------------------------- */
169 
170 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPtu46 & /* constant */)
171 {
172  os << "Geometric parameters: " << std::endl
173  << "L: "
174  << "\t" << vpPtu46::L << std::endl
175  << "h: "
176  << "\t" << vpPtu46::h << std::endl;
177 
178  return os;
179 }
180 
192 {
194  get_cMe(cMe);
195 
196  cVe.buildFrom(cMe);
197 }
198 
209 {
211 
212  eMc[0][0] = 0;
213  eMc[0][1] = -1;
214  eMc[0][2] = 0;
215  eMc[0][3] = h;
216 
217  eMc[1][0] = 1;
218  eMc[1][1] = 0;
219  eMc[1][2] = 0;
220  eMc[1][3] = -L;
221 
222  eMc[2][0] = 0;
223  eMc[2][1] = 0;
224  eMc[2][2] = 1;
225  eMc[2][3] = 0;
226 
227  eMc[3][0] = 0;
228  eMc[3][1] = 0;
229  eMc[3][2] = 0;
230  eMc[3][3] = 1;
231 
232  cMe = eMc.inverse();
233 }
234 
247 void vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
248 {
249 
250  eJe.resize(6, 2);
251 
252  if (q.getRows() != 2) {
253  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
254  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
255  }
256 
257  double s2 = sin(q[1]);
258  double c2 = cos(q[1]);
259 
260  eJe = 0;
261 
262  eJe[3][0] = c2;
263  eJe[4][1] = 1;
264  eJe[5][0] = s2;
265 }
266 
276 void vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe) const
277 {
278 
279  if (q.getRows() != 2) {
280  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
281  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
282  }
283 
284  fJe.resize(6, 2);
285 
286  double s1 = sin(q[0]);
287  double c1 = cos(q[0]);
288 
289  fJe = 0;
290 
291  fJe[3][1] = s1;
292  fJe[4][1] = -c1;
293  fJe[5][0] = 1;
294 }
295 END_VISP_NAMESPACE
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:614
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
vpPoseVector & buildFrom(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz)
Jacobian, geometric model functionalities... for ptu46, pan, tilt head from Directed Perception.
Definition: vpPtu46.h:72
vpPtu46(void)
Definition: vpPtu46.cpp:155
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Definition: vpPtu46.cpp:191
static const unsigned int ndof
Definition: vpPtu46.h:76
void init(void)
Definition: vpPtu46.cpp:164
void get_cMe(vpHomogeneousMatrix &_cMe) const
Definition: vpPtu46.cpp:208
static const float L
Definition: vpPtu46.h:79
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpPtu46.cpp:68
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpPtu46.cpp:276
static const float h
Definition: vpPtu46.h:80
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpPtu46.cpp:247
vpVelocityTwistMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)