Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpBiclops.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 Biclops robot.
33 *
34*****************************************************************************/
35
36/* ----------------------------------------------------------------------- */
37/* --- INCLUDE ----------------------------------------------------------- */
38/* ----------------------------------------------------------------------- */
39
40#include <math.h>
41#include <visp3/core/vpDebug.h>
42#include <visp3/core/vpMath.h>
43#include <visp3/robot/vpBiclops.h>
44#include <visp3/robot/vpRobotException.h>
45
46/* ------------------------------------------------------------------------ */
47/* --- COMPUTE ------------------------------------------------------------ */
48/* ------------------------------------------------------------------------ */
49const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
50const float vpBiclops::h = 0.048f; /*<! Vertical offset from last joint to camera frame. */
51const float vpBiclops::panJointLimit = (float)(M_PI);
53const float vpBiclops::tiltJointLimit = (float)(M_PI / 4.5);
56const float vpBiclops::speedLimit = (float)(M_PI / 3.0);
73{
75 fMc = fMe * cMe_.inverse();
76
77 vpCDEBUG(6) << "camera position: " << std::endl << fMc;
78
79 return;
80}
81
93{
95 fMc = fMe * cMe_.inverse();
96
97 vpCDEBUG(6) << "camera position: " << std::endl << fMc;
98
99 return;
100}
101
117{
119
120 computeMGD(q, fMc);
121
122 return fMc;
123}
124
136{
138
139 get_fMc(q, fMc);
140
141 return fMc;
142}
143
155{
157
158 if (q.getRows() != 2) {
159 vpERROR_TRACE("Bad dimension for biclops articular vector");
160 throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
161 }
162
163 double q1 = q[0]; // pan
164 double q2 = q[1]; // tilt
165
166 double c1 = cos(q1);
167 double s1 = sin(q1);
168 double c2 = cos(q2);
169 double s2 = sin(q2);
170
171 if (dh_model_ == DH1) {
172 fMe[0][0] = -c1 * s2;
173 fMe[0][1] = -s1;
174 fMe[0][2] = c1 * c2;
175 fMe[0][3] = 0;
176
177 fMe[1][0] = -s1 * s2;
178 fMe[1][1] = c1;
179 fMe[1][2] = s1 * c2;
180 fMe[1][3] = 0;
181
182 fMe[2][0] = -c2;
183 fMe[2][1] = 0;
184 fMe[2][2] = -s2;
185 fMe[2][3] = 0;
186
187 fMe[3][0] = 0;
188 fMe[3][1] = 0;
189 fMe[3][2] = 0;
190 fMe[3][3] = 1;
191 } else {
192 fMe[0][0] = c1 * s2;
193 fMe[0][1] = -s1;
194 fMe[0][2] = c1 * c2;
195 fMe[0][3] = 0;
196
197 fMe[1][0] = s1 * s2;
198 fMe[1][1] = c1;
199 fMe[1][2] = s1 * c2;
200 fMe[1][3] = 0;
201
202 fMe[2][0] = -c2;
203 fMe[2][1] = 0;
204 fMe[2][2] = s2;
205 fMe[2][3] = 0;
206
207 fMe[3][0] = 0;
208 fMe[3][1] = 0;
209 fMe[3][2] = 0;
210 fMe[3][3] = 1;
211 }
212
213 return fMe;
214}
215
231{
233
234 get_fMc(q, fMc);
235 fvc.buildFrom(fMc.inverse());
236
237 return;
238}
239
251{
253
254 get_fMc(q, fMc);
255 fvc.buildFrom(fMc.inverse());
256
257 return;
258}
259
260/* ---------------------------------------------------------------------- */
261/* --- CONSTRUCTOR ------------------------------------------------------ */
262/* ---------------------------------------------------------------------- */
263
269vpBiclops::vpBiclops(void) : dh_model_(DH1), cMe_() { init(); }
270/* ---------------------------------------------------------------------- */
271/* --- PRIVATE ---------------------------------------------------------- */
272/* ---------------------------------------------------------------------- */
273
280{
281 dh_model_ = DH1;
282 set_cMe();
283 return;
284}
285
286/* ----------------------------------------------------------------------- */
287/* --- DISPLAY ----------------------------------------------------------- */
288/* ----------------------------------------------------------------------- */
289
290VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops & /*constant*/)
291{
292 os << "Geometric parameters: " << std::endl
293 << "h: "
294 << "\t" << vpBiclops::h << std::endl;
295
296 return os;
297}
298
310
319{
321
322 eMc[0][0] = 0;
323 eMc[0][1] = -1;
324 eMc[0][2] = 0;
325 eMc[0][3] = h;
326
327 eMc[1][0] = 1;
328 eMc[1][1] = 0;
329 eMc[1][2] = 0;
330 eMc[1][3] = 0;
331
332 eMc[2][0] = 0;
333 eMc[2][1] = 0;
334 eMc[2][2] = 1;
335 eMc[2][3] = 0;
336
337 eMc[3][0] = 0;
338 eMc[3][1] = 0;
339 eMc[3][2] = 0;
340 eMc[3][3] = 1;
341
342 cMe_ = eMc.inverse();
343}
344
357void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const
358{
359 eJe.resize(6, 2);
360
361 if (q.getRows() != 2) {
362 vpERROR_TRACE("Bad dimension for biclops articular vector");
363 throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
364 }
365
366 double s2 = sin(q[1]);
367 double c2 = cos(q[1]);
368
369 eJe = 0;
370
371 if (dh_model_ == DH1) {
372 eJe[3][0] = -c2;
373 eJe[4][1] = 1;
374 eJe[5][0] = -s2;
375 } else {
376 eJe[3][0] = -c2;
377 eJe[4][1] = -1;
378 eJe[5][0] = s2;
379 }
380}
391void vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const
392{
393 if (q.getRows() != 2) {
394 vpERROR_TRACE("Bad dimension for biclops articular vector");
395 throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
396 }
397
398 fJe.resize(6, 2);
399
400 double s1 = sin(q[0]);
401 double c1 = cos(q[0]);
402
403 fJe = 0;
404
405 if (dh_model_ == DH1) {
406 fJe[3][1] = -s1;
407 fJe[4][1] = c1;
408 fJe[5][0] = 1;
409 } else {
410 fJe[3][1] = s1;
411 fJe[4][1] = -c1;
412 fJe[5][0] = 1;
413 }
414}
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
Jacobian, geometric model functionalities... for biclops, pan, tilt head.
Definition vpBiclops.h:75
vpHomogeneousMatrix cMe_
Definition vpBiclops.h:134
static const float h
Definition vpBiclops.h:126
void init(void)
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition vpBiclops.cpp:72
void set_cMe()
static const float speedLimit
Definition vpBiclops.h:130
vpBiclops(void)
void get_cVe(vpVelocityTwistMatrix &_cVe) const
static const float tiltJointLimit
Definition vpBiclops.h:129
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition vpBiclops.cpp:92
vpHomogeneousMatrix get_fMe(const vpColVector &q) const
static const float panJointLimit
Definition vpBiclops.h:128
DenavitHartenbergModel dh_model_
Definition vpBiclops.h:133
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
static const unsigned int ndof
Definition vpBiclops.h:123
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)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition vpDebug.h:506
#define vpERROR_TRACE
Definition vpDebug.h:388