Visual Servoing Platform  version 3.6.1 under development (2024-04-18)
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 const unsigned int vpBiclops::ndof = 2;
41 const float vpBiclops::h = 0.048f;
42 const float vpBiclops::panJointLimit = (float)(M_PI);
43 const float vpBiclops::tiltJointLimit = (float)(M_PI / 4.5);
44 const float vpBiclops::speedLimit = (float)(M_PI / 3.0);
45 
47 {
49  fMc = fMe * m_cMe.inverse();
50 
51  vpCDEBUG(6) << "camera position: " << std::endl << fMc;
52 
53  return;
54 }
55 
57 {
59  fMc = fMe * m_cMe.inverse();
60 
61  vpCDEBUG(6) << "camera position: " << std::endl << fMc;
62 
63  return;
64 }
65 
67 {
69 
70  computeMGD(q, fMc);
71 
72  return fMc;
73 }
74 
76 {
78 
79  get_fMc(q, fMc);
80 
81  return fMc;
82 }
83 
85 {
87 
88  if (q.getRows() != 2) {
89  throw(vpException(vpException::dimensionError, "Bad dimension for Biclops joint position vector"));
90  }
91 
92  double q1 = q[0]; // pan
93  double q2 = q[1]; // tilt
94 
95  double c1 = cos(q1);
96  double s1 = sin(q1);
97  double c2 = cos(q2);
98  double s2 = sin(q2);
99 
100  if (m_dh_model == DH1) {
101  fMe[0][0] = -c1 * s2;
102  fMe[0][1] = -s1;
103  fMe[0][2] = c1 * c2;
104  fMe[0][3] = 0;
105 
106  fMe[1][0] = -s1 * s2;
107  fMe[1][1] = c1;
108  fMe[1][2] = s1 * c2;
109  fMe[1][3] = 0;
110 
111  fMe[2][0] = -c2;
112  fMe[2][1] = 0;
113  fMe[2][2] = -s2;
114  fMe[2][3] = 0;
115 
116  fMe[3][0] = 0;
117  fMe[3][1] = 0;
118  fMe[3][2] = 0;
119  fMe[3][3] = 1;
120  }
121  else {
122  fMe[0][0] = c1 * s2;
123  fMe[0][1] = -s1;
124  fMe[0][2] = c1 * c2;
125  fMe[0][3] = 0;
126 
127  fMe[1][0] = s1 * s2;
128  fMe[1][1] = c1;
129  fMe[1][2] = s1 * c2;
130  fMe[1][3] = 0;
131 
132  fMe[2][0] = -c2;
133  fMe[2][1] = 0;
134  fMe[2][2] = s2;
135  fMe[2][3] = 0;
136 
137  fMe[3][0] = 0;
138  fMe[3][1] = 0;
139  fMe[3][2] = 0;
140  fMe[3][3] = 1;
141  }
142 
143  return fMe;
144 }
145 
147 {
149 
150  get_fMc(q, fMc);
151  fPc.buildFrom(fMc.inverse());
152 
153  return;
154 }
155 
156 void vpBiclops::get_fMc(const vpColVector &q, vpPoseVector &fPc) const
157 {
159 
160  get_fMc(q, fMc);
161  fPc.buildFrom(fMc.inverse());
162 
163  return;
164 }
165 
166 vpBiclops::vpBiclops(void) : m_dh_model(DH1), m_cMe() { init(); }
167 
169 {
170  m_dh_model = DH1;
171  set_cMe();
172  return;
173 }
174 
175 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops & /*constant*/)
176 {
177  os << "Geometric parameters: " << std::endl
178  << "h: "
179  << "\t" << vpBiclops::h << std::endl;
180 
181  return os;
182 }
183 
185 
187 {
189 
190  m_cMe[0][0] = 0;
191  m_cMe[0][1] = 1;
192  m_cMe[0][2] = 0;
193  m_cMe[0][3] = 0;
194 
195  m_cMe[1][0] = -1;
196  m_cMe[1][1] = 0;
197  m_cMe[1][2] = 0;
198  m_cMe[1][3] = h;
199 
200  m_cMe[2][0] = 0;
201  m_cMe[2][1] = 0;
202  m_cMe[2][2] = 1;
203  m_cMe[2][3] = 0;
204 
205  m_cMe[3][0] = 0;
206  m_cMe[3][1] = 0;
207  m_cMe[3][2] = 0;
208  m_cMe[3][3] = 1;
209 }
210 
211 void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const
212 {
213  eJe.resize(6, 2);
214 
215  if (q.getRows() != 2) {
216  throw(vpException(vpException::dimensionError, "Bad dimension for Biclops joint position vector"));
217  }
218 
219  double s2 = sin(q[1]);
220  double c2 = cos(q[1]);
221 
222  eJe = 0;
223 
224  if (m_dh_model == DH1) {
225  eJe[3][0] = -c2;
226  eJe[4][1] = 1;
227  eJe[5][0] = -s2;
228  }
229  else {
230  eJe[3][0] = -c2;
231  eJe[4][1] = -1;
232  eJe[5][0] = s2;
233  }
234 }
235 
236 void vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const
237 {
238  if (q.getRows() != 2) {
239  throw(vpException(vpException::dimensionError, "Bad dimension for Biclops joint position vector"));
240  }
241 
242  fJe.resize(6, 2);
243 
244  double s1 = sin(q[0]);
245  double c1 = cos(q[0]);
246 
247  fJe = 0;
248 
249  if (m_dh_model == DH1) {
250  fJe[3][1] = -s1;
251  fJe[4][1] = c1;
252  fJe[5][0] = 1;
253  }
254  else {
255  fJe[3][1] = s1;
256  fJe[4][1] = -c1;
257  fJe[5][0] = 1;
258  }
259 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:339
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:587
unsigned int getRows() const
Definition: vpArray2D.h:324
Jacobian, geometric model functionalities... for Biclops, pan, tilt head.
Definition: vpBiclops.h:62
static const float h
Definition: vpBiclops.h:102
vpHomogeneousMatrix m_cMe
Camera frame to PT end-effector frame transformation.
Definition: vpBiclops.h:109
void init(void)
Definition: vpBiclops.cpp:168
void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:46
void set_cMe()
Definition: vpBiclops.cpp:186
static const float speedLimit
Pan and tilt axis max velocity in rad/s to perform a displacement.
Definition: vpBiclops.h:105
vpBiclops(void)
Definition: vpBiclops.cpp:166
static const float tiltJointLimit
Tilt axis +/- joint limit in rad.
Definition: vpBiclops.h:104
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:236
DenavitHartenbergModel m_dh_model
Denavit-Hartenberg model.
Definition: vpBiclops.h:108
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:56
vpHomogeneousMatrix get_fMe(const vpColVector &q) const
Definition: vpBiclops.cpp:84
static const float panJointLimit
Pan axis +/- joint limit in rad.
Definition: vpBiclops.h:103
@ DH1
First Denavit-Hartenberg representation.
Definition: vpBiclops.h:94
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpBiclops.cpp:184
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:211
static const unsigned int ndof
Number of dof.
Definition: vpBiclops.h:99
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
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:146
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
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:497