Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
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 /* ------------------------------------------------------------------------ */
49 /* --- COMPUTE ------------------------------------------------------------ */
50 /* ------------------------------------------------------------------------ */
51 const unsigned int vpPtu46::ndof = 2; /*<! Pan and tilt are considered. */
52 const float vpPtu46::L = 0.0765f;
54 const float vpPtu46::h = 0.068f; /*<! Vertical offset from last joint to
55  camera frame. */
56 
68 {
69  if (q.getRows() != 2) {
70  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
71  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
72  }
73 
74  double q1 = q[0]; // pan
75  double q2 = q[1]; // tilt
76 
77  double c1 = cos(q1);
78  double s1 = sin(q1);
79  double c2 = cos(q2);
80  double s2 = sin(q2);
81 
82  fMc[0][0] = s1;
83  fMc[0][1] = c1 * s2;
84  fMc[0][2] = c1 * c2;
85  fMc[0][3] = -h * c1 * s2 - L * s1;
86 
87  fMc[1][0] = -c1;
88  fMc[1][1] = s1 * s2;
89  fMc[1][2] = s1 * c2;
90  fMc[1][3] = -h * s1 * s2 + L * c1;
91 
92  fMc[2][0] = 0;
93  fMc[2][1] = -c2;
94  fMc[2][2] = s2;
95  fMc[2][3] = h * c2;
96 
97  fMc[3][0] = 0;
98  fMc[3][1] = 0;
99  fMc[3][2] = 0;
100  fMc[3][3] = 1;
101 
102  vpCDEBUG(6) << "Position de la camera: " << std::endl << fMc;
103 
104  return;
105 }
106 
118 {
120 
121  computeMGD(q, fMc);
122 
123  return fMc;
124 }
125 
136 {
138 
139  computeMGD(q, fMc);
140  r.buildFrom(fMc.inverse());
141 
142  return;
143 }
144 
145 /* ---------------------------------------------------------------------- */
146 /* --- CONSTRUCTOR ------------------------------------------------------ */
147 /* ---------------------------------------------------------------------- */
148 
154 vpPtu46::vpPtu46(void) { init(); }
155 /* ---------------------------------------------------------------------- */
156 /* --- PRIVATE ---------------------------------------------------------- */
157 /* ---------------------------------------------------------------------- */
158 
163 void vpPtu46::init() { return; }
164 
165 /* ----------------------------------------------------------------------- */
166 /* --- DISPLAY ----------------------------------------------------------- */
167 /* ----------------------------------------------------------------------- */
168 
169 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPtu46 & /* constant */)
170 {
171  os << "Geometric parameters: " << std::endl
172  << "L: "
173  << "\t" << vpPtu46::L << std::endl
174  << "h: "
175  << "\t" << vpPtu46::h << std::endl;
176 
177  return os;
178 }
179 
191 {
193  get_cMe(cMe);
194 
195  cVe.buildFrom(cMe);
196 }
197 
208 {
210 
211  eMc[0][0] = 0;
212  eMc[0][1] = -1;
213  eMc[0][2] = 0;
214  eMc[0][3] = h;
215 
216  eMc[1][0] = 1;
217  eMc[1][1] = 0;
218  eMc[1][2] = 0;
219  eMc[1][3] = -L;
220 
221  eMc[2][0] = 0;
222  eMc[2][1] = 0;
223  eMc[2][2] = 1;
224  eMc[2][3] = 0;
225 
226  eMc[3][0] = 0;
227  eMc[3][1] = 0;
228  eMc[3][2] = 0;
229  eMc[3][3] = 1;
230 
231  cMe = eMc.inverse();
232 }
233 
246 void vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
247 {
248 
249  eJe.resize(6, 2);
250 
251  if (q.getRows() != 2) {
252  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
253  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
254  }
255 
256  double s2 = sin(q[1]);
257  double c2 = cos(q[1]);
258 
259  eJe = 0;
260 
261  eJe[3][0] = c2;
262  eJe[4][1] = 1;
263  eJe[5][0] = s2;
264 }
265 
275 void vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe) const
276 {
277 
278  if (q.getRows() != 2) {
279  vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
280  throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
281  }
282 
283  fJe.resize(6, 2);
284 
285  double s1 = sin(q[0]);
286  double c1 = cos(q[0]);
287 
288  fJe = 0;
289 
290  fJe[3][1] = s1;
291  fJe[4][1] = -c1;
292  fJe[5][0] = 1;
293 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:547
unsigned int getRows() const
Definition: vpArray2D.h:284
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
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:146
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Jacobian, geometric model functionalities... for ptu46, pan, tilt head from Directed Perception.
Definition: vpPtu46.h:71
vpPtu46(void)
Definition: vpPtu46.cpp:154
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Definition: vpPtu46.cpp:190
void init(void)
Definition: vpPtu46.cpp:163
void get_cMe(vpHomogeneousMatrix &_cMe) const
Definition: vpPtu46.cpp:207
static const float L
Definition: vpPtu46.h:78
static const unsigned int ndof
Definition: vpPtu46.h:75
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpPtu46.cpp:67
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpPtu46.cpp:275
static const float h
Definition: vpPtu46.h:79
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpPtu46.cpp:246
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition: vpDebug.h:497
#define vpERROR_TRACE
Definition: vpDebug.h:382