Visual Servoing Platform version 3.5.0
vpBiclops.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 Biclops robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39/* ----------------------------------------------------------------------- */
40/* --- INCLUDE ----------------------------------------------------------- */
41/* ----------------------------------------------------------------------- */
42
43#include <math.h>
44#include <visp3/core/vpDebug.h>
45#include <visp3/core/vpMath.h>
46#include <visp3/robot/vpBiclops.h>
47#include <visp3/robot/vpRobotException.h>
48
49/* ------------------------------------------------------------------------ */
50/* --- COMPUTE ------------------------------------------------------------ */
51/* ------------------------------------------------------------------------ */
52const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
53const float vpBiclops::h = 0.048f; /*<! Vertical offset from last joint to camera frame. */
54const float vpBiclops::panJointLimit = (float)(M_PI);
56const float vpBiclops::tiltJointLimit = (float)(M_PI / 4.5);
59const float vpBiclops::speedLimit = (float)(M_PI / 3.0);
76{
78 fMc = fMe * cMe_.inverse();
79
80 vpCDEBUG(6) << "camera position: " << std::endl << fMc;
81
82 return;
83}
84
96{
98 fMc = fMe * cMe_.inverse();
99
100 vpCDEBUG(6) << "camera position: " << std::endl << fMc;
101
102 return;
103}
104
120{
122
123 computeMGD(q, fMc);
124
125 return fMc;
126}
127
139{
141
142 get_fMc(q, fMc);
143
144 return fMc;
145}
146
158{
160
161 if (q.getRows() != 2) {
162 vpERROR_TRACE("Bad dimension for biclops articular vector");
163 throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
164 }
165
166 double q1 = q[0]; // pan
167 double q2 = q[1]; // tilt
168
169 double c1 = cos(q1);
170 double s1 = sin(q1);
171 double c2 = cos(q2);
172 double s2 = sin(q2);
173
174 if (dh_model_ == DH1) {
175 fMe[0][0] = -c1 * s2;
176 fMe[0][1] = -s1;
177 fMe[0][2] = c1 * c2;
178 fMe[0][3] = 0;
179
180 fMe[1][0] = -s1 * s2;
181 fMe[1][1] = c1;
182 fMe[1][2] = s1 * c2;
183 fMe[1][3] = 0;
184
185 fMe[2][0] = -c2;
186 fMe[2][1] = 0;
187 fMe[2][2] = -s2;
188 fMe[2][3] = 0;
189
190 fMe[3][0] = 0;
191 fMe[3][1] = 0;
192 fMe[3][2] = 0;
193 fMe[3][3] = 1;
194 } else {
195 fMe[0][0] = c1 * s2;
196 fMe[0][1] = -s1;
197 fMe[0][2] = c1 * c2;
198 fMe[0][3] = 0;
199
200 fMe[1][0] = s1 * s2;
201 fMe[1][1] = c1;
202 fMe[1][2] = s1 * c2;
203 fMe[1][3] = 0;
204
205 fMe[2][0] = -c2;
206 fMe[2][1] = 0;
207 fMe[2][2] = s2;
208 fMe[2][3] = 0;
209
210 fMe[3][0] = 0;
211 fMe[3][1] = 0;
212 fMe[3][2] = 0;
213 fMe[3][3] = 1;
214 }
215
216 return fMe;
217}
218
234{
236
237 get_fMc(q, fMc);
238 fvc.buildFrom(fMc.inverse());
239
240 return;
241}
242
254{
256
257 get_fMc(q, fMc);
258 fvc.buildFrom(fMc.inverse());
259
260 return;
261}
262
263/* ---------------------------------------------------------------------- */
264/* --- CONSTRUCTOR ------------------------------------------------------ */
265/* ---------------------------------------------------------------------- */
266
272vpBiclops::vpBiclops(void) : dh_model_(DH1), cMe_() { init(); }
273/* ---------------------------------------------------------------------- */
274/* --- PRIVATE ---------------------------------------------------------- */
275/* ---------------------------------------------------------------------- */
276
283{
284 dh_model_ = DH1;
285 set_cMe();
286 return;
287}
288
289/* ----------------------------------------------------------------------- */
290/* --- DISPLAY ----------------------------------------------------------- */
291/* ----------------------------------------------------------------------- */
292
293VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops & /*constant*/)
294{
295 os << "Geometric parameters: " << std::endl
296 << "h: "
297 << "\t" << vpBiclops::h << std::endl;
298
299 return os;
300}
301
313
322{
324
325 eMc[0][0] = 0;
326 eMc[0][1] = -1;
327 eMc[0][2] = 0;
328 eMc[0][3] = h;
329
330 eMc[1][0] = 1;
331 eMc[1][1] = 0;
332 eMc[1][2] = 0;
333 eMc[1][3] = 0;
334
335 eMc[2][0] = 0;
336 eMc[2][1] = 0;
337 eMc[2][2] = 1;
338 eMc[2][3] = 0;
339
340 eMc[3][0] = 0;
341 eMc[3][1] = 0;
342 eMc[3][2] = 0;
343 eMc[3][3] = 1;
344
345 cMe_ = eMc.inverse();
346}
347
360void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const
361{
362 eJe.resize(6, 2);
363
364 if (q.getRows() != 2) {
365 vpERROR_TRACE("Bad dimension for biclops articular vector");
366 throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
367 }
368
369 double s2 = sin(q[1]);
370 double c2 = cos(q[1]);
371
372 eJe = 0;
373
374 if (dh_model_ == DH1) {
375 eJe[3][0] = -c2;
376 eJe[4][1] = 1;
377 eJe[5][0] = -s2;
378 } else {
379 eJe[3][0] = -c2;
380 eJe[4][1] = -1;
381 eJe[5][0] = s2;
382 }
383}
394void vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const
395{
396 if (q.getRows() != 2) {
397 vpERROR_TRACE("Bad dimension for biclops articular vector");
398 throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
399 }
400
401 fJe.resize(6, 2);
402
403 double s1 = sin(q[0]);
404 double c1 = cos(q[0]);
405
406 fJe = 0;
407
408 if (dh_model_ == DH1) {
409 fJe[3][1] = -s1;
410 fJe[4][1] = c1;
411 fJe[5][0] = 1;
412 } else {
413 fJe[3][1] = s1;
414 fJe[4][1] = -c1;
415 fJe[5][0] = 1;
416 }
417}
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
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
Definition: vpBiclops.h:78
vpHomogeneousMatrix cMe_
Definition: vpBiclops.h:137
static const float h
Definition: vpBiclops.h:129
void init(void)
Definition: vpBiclops.cpp:282
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:75
void set_cMe()
Definition: vpBiclops.cpp:321
static const float speedLimit
Definition: vpBiclops.h:133
vpBiclops(void)
Definition: vpBiclops.cpp:272
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Definition: vpBiclops.cpp:312
static const float tiltJointLimit
Definition: vpBiclops.h:132
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:394
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:95
vpHomogeneousMatrix get_fMe(const vpColVector &q) const
Definition: vpBiclops.cpp:157
static const float panJointLimit
Definition: vpBiclops.h:131
DenavitHartenbergModel dh_model_
Definition: vpBiclops.h:136
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:360
static const unsigned int ndof
Definition: vpBiclops.h:126
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)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition: vpDebug.h:511
#define vpERROR_TRACE
Definition: vpDebug.h:393