ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
servoAfma4Point2DCamVelocity.cpp
1
/****************************************************************************
2
*
3
* $Id: servoAfma4Point2DCamVelocity.cpp 4056 2013-01-05 13:04:42Z fspindle $
4
*
5
* This file is part of the ViSP software.
6
* Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7
*
8
* This software is free software; you can redistribute it and/or
9
* modify it under the terms of the GNU General Public License
10
* ("GPL") version 2 as published by the Free Software Foundation.
11
* See the file LICENSE.txt at the root directory of this source
12
* distribution for additional information about the GNU GPL.
13
*
14
* For using ViSP with software that can not be combined with the GNU
15
* GPL, please contact INRIA about acquiring a ViSP Professional
16
* Edition License.
17
*
18
* See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19
*
20
* This software was developed at:
21
* INRIA Rennes - Bretagne Atlantique
22
* Campus Universitaire de Beaulieu
23
* 35042 Rennes Cedex
24
* France
25
* http://www.irisa.fr/lagadic
26
*
27
* If you have questions regarding the use of this file, please contact
28
* INRIA at visp@inria.fr
29
*
30
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32
*
33
*
34
* Description:
35
* tests the control law
36
* eye-in-hand control
37
* velocity computed in the camera frame
38
*
39
* Authors:
40
* Eric Marchand
41
* Fabien Spindler
42
*
43
*****************************************************************************/
44
69
#include <visp/vpConfig.h>
70
#include <visp/vpDebug.h>
// Debug trace
71
#include <stdlib.h>
72
#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394_2))
73
74
#include <visp/vp1394TwoGrabber.h>
75
#include <visp/vpImage.h>
76
#include <visp/vpImagePoint.h>
77
#include <visp/vpDisplay.h>
78
#include <visp/vpDisplayX.h>
79
#include <visp/vpDisplayOpenCV.h>
80
#include <visp/vpDisplayGTK.h>
81
82
#include <visp/vpMath.h>
83
#include <visp/vpHomogeneousMatrix.h>
84
#include <visp/vpFeaturePoint.h>
85
#include <visp/vpPoint.h>
86
#include <visp/vpServo.h>
87
#include <visp/vpFeatureBuilder.h>
88
#include <visp/vpRobotAfma4.h>
89
#include <visp/vpIoTools.h>
90
91
// Exception
92
#include <visp/vpException.h>
93
#include <visp/vpMatrixException.h>
94
#include <visp/vpServoDisplay.h>
95
96
#include <visp/vpDot.h>
97
98
int
99
main()
100
{
101
// Log file creation in /tmp/$USERNAME/log.dat
102
// This file contains by line:
103
// - the 6 computed cam velocities (m/s, rad/s) to achieve the task
104
// - the 6 mesured joint velocities (m/s, rad/s)
105
// - the 6 mesured joint positions (m, rad)
106
// - the 2 values of s - s*
107
std::string username;
108
// Get the user login name
109
vpIoTools::getUserName
(username);
110
111
// Create a log filename to save velocities...
112
std::string logdirname;
113
logdirname =
"/tmp/"
+ username;
114
115
// Test if the output path exist. If no try to create it
116
if
(
vpIoTools::checkDirectory
(logdirname) ==
false
) {
117
try
{
118
// Create the dirname
119
vpIoTools::makeDirectory
(logdirname);
120
}
121
catch
(...) {
122
std::cerr << std::endl
123
<<
"ERROR:"
<< std::endl;
124
std::cerr <<
" Cannot create "
<< logdirname << std::endl;
125
exit(-1);
126
}
127
}
128
std::string logfilename;
129
logfilename = logdirname +
"/log.dat"
;
130
131
// Open the log file name
132
std::ofstream flog(logfilename.c_str());
133
134
try
135
{
136
vpRobotAfma4
robot ;
137
vpServo
task ;
138
139
140
vpImage<unsigned char>
I ;
141
142
vp1394TwoGrabber
g(
false
);
143
g.setVideoMode(
vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8
);
144
g.setFramerate(
vp1394TwoGrabber::vpFRAMERATE_60
);
145
g.open(I) ;
146
147
g.acquire(I) ;
148
149
#ifdef VISP_HAVE_X11
150
vpDisplayX
display(I,100,100,
"Current image"
) ;
151
#elif defined(VISP_HAVE_OPENCV)
152
vpDisplayOpenCV
display(I,100,100,
"Current image"
) ;
153
#elif defined(VISP_HAVE_GTK)
154
vpDisplayGTK
display(I,100,100,
"Current image"
) ;
155
#endif
156
157
vpDisplay::display
(I) ;
158
vpDisplay::flush
(I) ;
159
160
std::cout << std::endl ;
161
std::cout <<
"-------------------------------------------------------"
<< std::endl ;
162
std::cout <<
" Test program for vpServo "
<<std::endl ;
163
std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame"
<< std::endl ;
164
std::cout <<
" Simulation "
<< std::endl ;
165
std::cout <<
" task : servo a point "
<< std::endl ;
166
std::cout <<
"-------------------------------------------------------"
<< std::endl ;
167
std::cout << std::endl ;
168
169
vpDot
dot ;
170
171
std::cout <<
"Click on a dot..."
<< std::endl;
172
dot.
initTracking
(I) ;
173
174
// Get the cog of the dot
175
vpImagePoint
cog = dot.
getCog
();
176
177
vpDisplay::displayCross
(I, cog, 10,
vpColor::blue
) ;
178
vpDisplay::flush
(I);
179
180
181
vpCameraParameters
cam ;
182
183
vpTRACE
(
"sets the current position of the visual feature "
) ;
184
vpFeaturePoint
p ;
185
vpFeatureBuilder::create
(p, cam, dot) ;
//retrieve x,y and Z of the vpPoint structure
186
187
vpTRACE
(
"sets the desired position of the visual feature "
) ;
188
vpFeaturePoint
pd ;
189
pd.
buildFrom
(0,0,1) ;
190
191
vpTRACE
(
"define the task"
) ;
192
vpTRACE
(
"\t we want an eye-in-hand control law"
) ;
193
vpTRACE
(
"\t robot is controlled in the camera frame"
) ;
194
task.
setServo
(
vpServo::EYEINHAND_CAMERA
) ;
195
196
vpTRACE
(
"\t we want to see a point on a point.."
) ;
197
std::cout << std::endl ;
198
task.
addFeature
(p,pd) ;
199
200
vpTRACE
(
"\t set the gain"
) ;
201
task.
setLambda
(0.8) ;
202
203
204
vpTRACE
(
"Display task information "
) ;
205
task.
print
() ;
206
207
208
robot.
setRobotState
(
vpRobot::STATE_VELOCITY_CONTROL
) ;
209
210
std::cout <<
"\nHit CTRL-C to stop the loop...\n"
<< std::flush;
211
for
( ; ; ) {
212
// Acquire a new image from the camera
213
g.acquire(I) ;
214
215
// Display this image
216
vpDisplay::display
(I) ;
217
218
// Achieve the tracking of the dot in the image
219
dot.
track
(I) ;
220
221
// Get the cog of the dot
222
vpImagePoint
cog = dot.
getCog
();
223
224
// Display a green cross at the center of gravity position in the image
225
vpDisplay::displayCross
(I, cog, 10,
vpColor::green
) ;
226
227
// Update the point feature from the dot location
228
vpFeatureBuilder::create
(p, cam, dot);
229
230
vpColVector
v ;
231
// Compute the visual servoing skew vector
232
v = task.
computeControlLaw
() ;
233
234
// Display the current and desired feature points in the image display
235
vpServoDisplay::display
(task, cam, I) ;
236
237
// Apply the computed joint velocities to the robot
238
robot.
setVelocity
(
vpRobot::CAMERA_FRAME
, v) ;
239
240
// Save velocities applied to the robot in the log file
241
// v[0], v[1], v[2] correspond to camera translation velocities in m/s
242
// v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
243
flog << v[0] <<
" "
<< v[1] <<
" "
<< v[2] <<
" "
244
<< v[3] <<
" "
<< v[4] <<
" "
<< v[5] <<
" "
;
245
246
// Get the measured joint velocities of the robot
247
vpColVector
qvel;
248
robot.
getVelocity
(
vpRobot::ARTICULAR_FRAME
, qvel);
249
// Save measured joint velocities of the robot in the log file:
250
// - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
251
// velocities in m/s
252
// - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
253
// velocities in rad/s
254
flog << qvel[0] <<
" "
<< qvel[1] <<
" "
<< qvel[2] <<
" "
255
<< qvel[3] <<
" "
<< qvel[4] <<
" "
<< qvel[5] <<
" "
;
256
257
// Get the measured joint positions of the robot
258
vpColVector
q;
259
robot.
getPosition
(
vpRobot::ARTICULAR_FRAME
, q);
260
// Save measured joint positions of the robot in the log file
261
// - q[0], q[1], q[2] correspond to measured joint translation
262
// positions in m
263
// - q[3], q[4], q[5] correspond to measured joint rotation
264
// positions in rad
265
flog << q[0] <<
" "
<< q[1] <<
" "
<< q[2] <<
" "
266
<< q[3] <<
" "
<< q[4] <<
" "
<< q[5] <<
" "
;
267
268
// Save feature error (s-s*) for the feature point. For this feature
269
// point, we have 2 errors (along x and y axis). This error is expressed
270
// in meters in the camera frame
271
flog << task.
getError
() << std::endl;
272
273
// Flush the display
274
vpDisplay::flush
(I) ;
275
276
}
277
278
flog.close() ;
// Close the log file
279
280
// Display task information
281
task.
print
() ;
282
283
// Kill the task
284
task.
kill
();
285
286
return
0;
287
}
288
catch
(...)
289
{
290
flog.close() ;
// Close the log file
291
vpERROR_TRACE
(
" Test failed"
) ;
292
return
0;
293
}
294
}
295
296
297
#else
298
int
299
main()
300
{
301
vpERROR_TRACE
(
"You do not have an afma4 robot or a firewire framegrabber connected to your computer..."
);
302
}
303
#endif
example
servo-afma4
servoAfma4Point2DCamVelocity.cpp
Generated on Thu Oct 24 2013 14:47:34 for ViSP by
1.8.4