ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
servoViper850Point2DCamVelocity.cpp
1
/****************************************************************************
2
*
3
* $Id: servoViper850Point2DCamVelocity.cpp 4065 2013-01-11 13:32:47Z 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 camera frame
38
*
39
* Authors:
40
* Eric Marchand
41
* Fabien Spindler
42
*
43
*****************************************************************************/
44
45
56
#include <visp/vpConfig.h>
57
#include <visp/vpDebug.h>
// Debug trace
58
59
#include <stdlib.h>
60
#include <stdio.h>
61
#include <iostream>
62
#include <fstream>
63
#include <sstream>
64
65
#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
66
67
#include <visp/vp1394TwoGrabber.h>
68
#include <visp/vpImage.h>
69
#include <visp/vpMath.h>
70
#include <visp/vpHomogeneousMatrix.h>
71
#include <visp/vpFeaturePoint.h>
72
#include <visp/vpPoint.h>
73
#include <visp/vpServo.h>
74
#include <visp/vpFeatureBuilder.h>
75
#include <visp/vpRobotViper850.h>
76
#include <visp/vpIoTools.h>
77
#include <visp/vpException.h>
78
#include <visp/vpMatrixException.h>
79
#include <visp/vpServoDisplay.h>
80
#include <visp/vpImageIo.h>
81
#include <visp/vpDot2.h>
82
#include <visp/vpDisplay.h>
83
#include <visp/vpDisplayX.h>
84
#include <visp/vpDisplayOpenCV.h>
85
#include <visp/vpDisplayGTK.h>
86
87
88
int
89
main()
90
{
91
// Log file creation in /tmp/$USERNAME/log.dat
92
// This file contains by line:
93
// - the 6 computed joint velocities (m/s, rad/s) to achieve the task
94
// - the 6 mesured joint velocities (m/s, rad/s)
95
// - the 6 mesured joint positions (m, rad)
96
// - the 2 values of s - s*
97
std::string username;
98
// Get the user login name
99
vpIoTools::getUserName
(username);
100
101
// Create a log filename to save velocities...
102
std::string logdirname;
103
logdirname =
"/tmp/"
+ username;
104
105
// Test if the output path exist. If no try to create it
106
if
(
vpIoTools::checkDirectory
(logdirname) ==
false
) {
107
try
{
108
// Create the dirname
109
vpIoTools::makeDirectory
(logdirname);
110
}
111
catch
(...) {
112
std::cerr << std::endl
113
<<
"ERROR:"
<< std::endl;
114
std::cerr <<
" Cannot create "
<< logdirname << std::endl;
115
exit(-1);
116
}
117
}
118
std::string logfilename;
119
logfilename = logdirname +
"/log.dat"
;
120
121
// Open the log file name
122
std::ofstream flog(logfilename.c_str());
123
124
try
{
125
vpRobotViper850
robot ;
126
127
vpServo
task ;
128
129
vpImage<unsigned char>
I ;
130
131
bool
reset =
false
;
132
vp1394TwoGrabber
g(reset);
133
134
#if 1
135
g.setVideoMode(
vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8
);
136
g.setFramerate(
vp1394TwoGrabber::vpFRAMERATE_60
);
137
#else
138
g.setVideoMode(
vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0
);
139
g.setColorCoding(
vp1394TwoGrabber::vpCOLOR_CODING_MONO8
);
140
#endif
141
g.open(I) ;
142
143
#ifdef VISP_HAVE_X11
144
vpDisplayX
display(I, (
int
)(100+I.
getWidth
()+30), 200,
"Current image"
) ;
145
#elif defined(VISP_HAVE_OPENCV)
146
vpDisplayOpenCV
display(I, (
int
)(100+I.
getWidth
()+30), 200,
"Current image"
) ;
147
#elif defined(VISP_HAVE_GTK)
148
vpDisplayGTK
display(I, (
int
)(100+I.
getWidth
()+30), 200,
"Current image"
) ;
149
#endif
150
151
vpDisplay::display
(I) ;
152
vpDisplay::flush
(I) ;
153
154
vpDot2
dot ;
155
vpImagePoint
cog;
156
157
dot.
setGraphics
(
true
);
158
159
for
(
int
i=0; i< 10; i++)
160
g.acquire(I) ;
161
162
std::cout <<
"Click on a dot..."
<< std::endl;
163
dot.
initTracking
(I) ;
164
165
cog = dot.
getCog
();
166
vpDisplay::displayCross
(I, cog, 10,
vpColor::blue
) ;
167
vpDisplay::flush
(I);
168
169
vpCameraParameters
cam ;
170
// Update camera parameters
171
robot.
getCameraParameters
(cam, I);
172
173
// sets the current position of the visual feature
174
vpFeaturePoint
p ;
175
// retrieve x,y and Z of the vpPoint structure
176
vpFeatureBuilder::create
(p,cam, dot);
177
178
// sets the desired position of the visual feature
179
vpFeaturePoint
pd ;
180
pd.
buildFrom
(0,0,1) ;
181
182
// define the task
183
// - we want an eye-in-hand control law
184
// - robot is controlled in the camera frame
185
task.
setServo
(
vpServo::EYEINHAND_CAMERA
) ;
186
187
// - we want to see a point on a point
188
task.
addFeature
(p,pd) ;
189
190
// - set the constant gain
191
task.
setLambda
(0.8) ;
192
193
// Display task information
194
task.
print
() ;
195
196
// Now the robot will be controlled in velocity
197
robot.
setRobotState
(
vpRobot::STATE_VELOCITY_CONTROL
) ;
198
199
std::cout <<
"\nHit CTRL-C to stop the loop...\n"
<< std::flush;
200
vpColVector
v ;
201
for
( ; ; ) {
202
try
{
203
// Acquire a new image from the camera
204
g.acquire(I) ;
205
206
// Display this image
207
vpDisplay::display
(I) ;
208
209
// Achieve the tracking of the dot in the image
210
dot.
track
(I) ;
211
212
// Get the dot cog
213
cog = dot.
getCog
();
214
215
// Display a green cross at the center of gravity position in the image
216
vpDisplay::displayCross
(I, cog, 10,
vpColor::green
) ;
217
218
// Update the point feature from the dot location
219
vpFeatureBuilder::create
(p, cam, dot);
220
221
// Compute the visual servoing skew vector
222
v = task.
computeControlLaw
() ;
223
224
// Display the current and desired feature points in the image display
225
vpServoDisplay::display
(task, cam, I) ;
226
227
// Apply the computed camera velocities to the robot
228
robot.
setVelocity
(
vpRobot::CAMERA_FRAME
, v) ;
229
}
230
catch
(...) {
231
std::cout <<
"Tracking failed... Stop the robot."
<< std::endl;
232
v = 0;
233
// Stop robot
234
robot.
setVelocity
(
vpRobot::CAMERA_FRAME
, v) ;
235
return
0;
236
}
237
238
// Save velocities applied to the robot in the log file
239
// v[0], v[1], v[2] correspond to camera translation velocities in m/s
240
// v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
241
flog << v[0] <<
" "
<< v[1] <<
" "
<< v[2] <<
" "
242
<< v[3] <<
" "
<< v[4] <<
" "
<< v[5] <<
" "
;
243
244
// Get the measured joint velocities of the robot
245
vpColVector
qvel;
246
robot.
getVelocity
(
vpRobot::ARTICULAR_FRAME
, qvel);
247
// Save measured joint velocities of the robot in the log file:
248
// - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
249
// velocities in m/s
250
// - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
251
// velocities in rad/s
252
flog << qvel[0] <<
" "
<< qvel[1] <<
" "
<< qvel[2] <<
" "
253
<< qvel[3] <<
" "
<< qvel[4] <<
" "
<< qvel[5] <<
" "
;
254
255
// Get the measured joint positions of the robot
256
vpColVector
q;
257
robot.
getPosition
(
vpRobot::ARTICULAR_FRAME
, q);
258
// Save measured joint positions of the robot in the log file
259
// - q[0], q[1], q[2] correspond to measured joint translation
260
// positions in m
261
// - q[3], q[4], q[5] correspond to measured joint rotation
262
// positions in rad
263
flog << q[0] <<
" "
<< q[1] <<
" "
<< q[2] <<
" "
264
<< q[3] <<
" "
<< q[4] <<
" "
<< q[5] <<
" "
;
265
266
// Save feature error (s-s*) for the feature point. For this feature
267
// point, we have 2 errors (along x and y axis). This error is expressed
268
// in meters in the camera frame
269
flog << ( task.
getError
() ).t() << std::endl;
// s-s* for point
270
271
// Flush the display
272
vpDisplay::flush
(I) ;
273
}
274
275
flog.close() ;
// Close the log file
276
277
// Display task information
278
task.
print
() ;
279
280
// Kill the task
281
task.
kill
();
282
283
return
0;
284
}
285
catch
(...)
286
{
287
flog.close() ;
// Close the log file
288
vpERROR_TRACE
(
" Test failed"
) ;
289
return
0;
290
}
291
}
292
293
#else
294
int
295
main()
296
{
297
vpERROR_TRACE
(
"You do not have a Viper robot or a firewire framegrabber connected to your computer..."
);
298
}
299
#endif
example
servo-viper850
servoViper850Point2DCamVelocity.cpp
Generated on Fri Apr 26 2013 19:54:32 for ViSP by
1.8.1.2