Visual Servoing Platform  version 3.6.1 under development (2024-10-13)
vpBiclops.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Interface for the Biclops robot.
32  */
33 
34 #include <math.h>
35 #include <visp3/core/vpDebug.h>
36 #include <visp3/core/vpMath.h>
37 #include <visp3/robot/vpBiclops.h>
38 #include <visp3/robot/vpRobotException.h>
39 
40 BEGIN_VISP_NAMESPACE
41 const unsigned int vpBiclops::ndof = 2;
42 const float vpBiclops::h = 0.048f;
43 const float vpBiclops::panJointLimit = (float)(M_PI);
44 const float vpBiclops::tiltJointLimit = (float)(M_PI / 4.5);
45 const float vpBiclops::speedLimit = (float)(M_PI / 3.0);
46 
48 {
50  fMc = fMe * m_cMe.inverse();
51 
52  vpCDEBUG(6) << "camera position: " << std::endl << fMc;
53 
54  return;
55 }
56 
58 {
60  fMc = fMe * m_cMe.inverse();
61 
62  vpCDEBUG(6) << "camera position: " << std::endl << fMc;
63 
64  return;
65 }
66 
68 {
70 
71  computeMGD(q, fMc);
72 
73  return fMc;
74 }
75 
77 {
79 
80  get_fMc(q, fMc);
81 
82  return fMc;
83 }
84 
86 {
88 
89  if (q.getRows() != 2) {
90  throw(vpException(vpException::dimensionError, "Bad dimension for Biclops joint position vector"));
91  }
92 
93  double q1 = q[0]; // pan
94  double q2 = q[1]; // tilt
95 
96  double c1 = cos(q1);
97  double s1 = sin(q1);
98  double c2 = cos(q2);
99  double s2 = sin(q2);
100 
101  if (m_dh_model == DH1) {
102  fMe[0][0] = -c1 * s2;
103  fMe[0][1] = -s1;
104  fMe[0][2] = c1 * c2;
105  fMe[0][3] = 0;
106 
107  fMe[1][0] = -s1 * s2;
108  fMe[1][1] = c1;
109  fMe[1][2] = s1 * c2;
110  fMe[1][3] = 0;
111 
112  fMe[2][0] = -c2;
113  fMe[2][1] = 0;
114  fMe[2][2] = -s2;
115  fMe[2][3] = 0;
116 
117  fMe[3][0] = 0;
118  fMe[3][1] = 0;
119  fMe[3][2] = 0;
120  fMe[3][3] = 1;
121  }
122  else {
123  fMe[0][0] = c1 * s2;
124  fMe[0][1] = -s1;
125  fMe[0][2] = c1 * c2;
126  fMe[0][3] = 0;
127 
128  fMe[1][0] = s1 * s2;
129  fMe[1][1] = c1;
130  fMe[1][2] = s1 * c2;
131  fMe[1][3] = 0;
132 
133  fMe[2][0] = -c2;
134  fMe[2][1] = 0;
135  fMe[2][2] = s2;
136  fMe[2][3] = 0;
137 
138  fMe[3][0] = 0;
139  fMe[3][1] = 0;
140  fMe[3][2] = 0;
141  fMe[3][3] = 1;
142  }
143 
144  return fMe;
145 }
146 
148 {
150 
151  get_fMc(q, fMc);
152  fPc.buildFrom(fMc.inverse());
153 
154  return;
155 }
156 
157 void vpBiclops::get_fMc(const vpColVector &q, vpPoseVector &fPc) const
158 {
160 
161  get_fMc(q, fMc);
162  fPc.buildFrom(fMc.inverse());
163 
164  return;
165 }
166 
167 vpBiclops::vpBiclops(void) : m_dh_model(DH1), m_cMe() { init(); }
168 
170 {
171  m_dh_model = DH1;
172  set_cMe();
173  return;
174 }
175 
176 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops & /*constant*/)
177 {
178  os << "Geometric parameters: " << std::endl
179  << "h: "
180  << "\t" << vpBiclops::h << std::endl;
181 
182  return os;
183 }
184 
186 
188 {
190 
191  m_cMe[0][0] = 0;
192  m_cMe[0][1] = 1;
193  m_cMe[0][2] = 0;
194  m_cMe[0][3] = 0;
195 
196  m_cMe[1][0] = -1;
197  m_cMe[1][1] = 0;
198  m_cMe[1][2] = 0;
199  m_cMe[1][3] = h;
200 
201  m_cMe[2][0] = 0;
202  m_cMe[2][1] = 0;
203  m_cMe[2][2] = 1;
204  m_cMe[2][3] = 0;
205 
206  m_cMe[3][0] = 0;
207  m_cMe[3][1] = 0;
208  m_cMe[3][2] = 0;
209  m_cMe[3][3] = 1;
210 }
211 
212 void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const
213 {
214  eJe.resize(6, 2);
215 
216  if (q.getRows() != 2) {
217  throw(vpException(vpException::dimensionError, "Bad dimension for Biclops joint position vector"));
218  }
219 
220  double s2 = sin(q[1]);
221  double c2 = cos(q[1]);
222 
223  eJe = 0;
224 
225  if (m_dh_model == DH1) {
226  eJe[3][0] = -c2;
227  eJe[4][1] = 1;
228  eJe[5][0] = -s2;
229  }
230  else {
231  eJe[3][0] = -c2;
232  eJe[4][1] = -1;
233  eJe[5][0] = s2;
234  }
235 }
236 
237 void vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const
238 {
239  if (q.getRows() != 2) {
240  throw(vpException(vpException::dimensionError, "Bad dimension for Biclops joint position vector"));
241  }
242 
243  fJe.resize(6, 2);
244 
245  double s1 = sin(q[0]);
246  double c1 = cos(q[0]);
247 
248  fJe = 0;
249 
250  if (m_dh_model == DH1) {
251  fJe[3][1] = -s1;
252  fJe[4][1] = c1;
253  fJe[5][0] = 1;
254  }
255  else {
256  fJe[3][1] = s1;
257  fJe[4][1] = -c1;
258  fJe[5][0] = 1;
259  }
260 }
261 END_VISP_NAMESPACE
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:611
unsigned int getRows() const
Definition: vpArray2D.h:347
Jacobian, geometric model functionalities... for Biclops, pan, tilt head.
Definition: vpBiclops.h:64
static const float h
Definition: vpBiclops.h:104
vpHomogeneousMatrix m_cMe
Camera frame to PT end-effector frame transformation.
Definition: vpBiclops.h:111
void init(void)
Definition: vpBiclops.cpp:169
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:47
void set_cMe()
Definition: vpBiclops.cpp:187
static const float speedLimit
Pan and tilt axis max velocity in rad/s to perform a displacement.
Definition: vpBiclops.h:107
vpBiclops(void)
Definition: vpBiclops.cpp:167
static const float tiltJointLimit
Tilt axis +/- joint limit in rad.
Definition: vpBiclops.h:106
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:237
DenavitHartenbergModel m_dh_model
Denavit-Hartenberg model.
Definition: vpBiclops.h:110
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:57
vpHomogeneousMatrix get_fMe(const vpColVector &q) const
Definition: vpBiclops.cpp:85
static const float panJointLimit
Pan axis +/- joint limit in rad.
Definition: vpBiclops.h:105
@ DH1
First Denavit-Hartenberg representation.
Definition: vpBiclops.h:96
static const unsigned int ndof
Number of dof.
Definition: vpBiclops.h:101
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpBiclops.cpp:185
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:212
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
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:169
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
vpPoseVector & buildFrom(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz)
vpVelocityTwistMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)