Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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/* ------------------------------------------------------------------------ */
51const unsigned int vpPtu46::ndof = 2; /*<! Pan and tilt are considered. */
52const float vpPtu46::L = 0.0765f;
54const 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
155/* ---------------------------------------------------------------------- */
156/* --- PRIVATE ---------------------------------------------------------- */
157/* ---------------------------------------------------------------------- */
158
163void vpPtu46::init() { return; }
164
165/* ----------------------------------------------------------------------- */
166/* --- DISPLAY ----------------------------------------------------------- */
167/* ----------------------------------------------------------------------- */
168
169VISP_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
246void 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
275void 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:305
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
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:152
Implementation of a pose vector and operations on poses.
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:506
#define vpERROR_TRACE
Definition vpDebug.h:388