Visual Servoing Platform version 3.5.0
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/* ------------------------------------------------------------------------ */
54const unsigned int vpPtu46::ndof = 2; /*<! Pan and tilt are considered. */
55const float vpPtu46::L = 0.0765f;
57const 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
158/* ---------------------------------------------------------------------- */
159/* --- PRIVATE ---------------------------------------------------------- */
160/* ---------------------------------------------------------------------- */
161
166void vpPtu46::init() { return; }
167
168/* ----------------------------------------------------------------------- */
169/* --- DISPLAY ----------------------------------------------------------- */
170/* ----------------------------------------------------------------------- */
171
172VISP_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
249void 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
278void 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}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
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:154
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Jacobian, geometric model functionnalities... for ptu46, pan, tilt head from Directed Perception.
Definition: vpPtu46.h:74
vpPtu46(void)
Definition: vpPtu46.cpp:157
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Definition: vpPtu46.cpp:193
void init(void)
Definition: vpPtu46.cpp:166
void get_cMe(vpHomogeneousMatrix &_cMe) const
Definition: vpPtu46.cpp:210
static const float L
Definition: vpPtu46.h:81
static const unsigned int ndof
Definition: vpPtu46.h:78
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpPtu46.cpp:70
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpPtu46.cpp:278
static const float h
Definition: vpPtu46.h:82
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpPtu46.cpp:249
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition: vpDebug.h:511
#define vpERROR_TRACE
Definition: vpDebug.h:393