ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp
1
/****************************************************************************
2
*
3
* $Id: servoAfma6FourPoints2DCamVelocityInteractionCurrent.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
70
#include <visp/vpConfig.h>
71
#include <visp/vpDebug.h>
// Debug trace
72
#include <stdlib.h>
73
#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
74
75
#include <visp/vp1394TwoGrabber.h>
76
#include <visp/vpImage.h>
77
#include <visp/vpImagePoint.h>
78
#include <visp/vpDisplay.h>
79
#include <visp/vpDisplayX.h>
80
#include <visp/vpDisplayOpenCV.h>
81
#include <visp/vpDisplayGTK.h>
82
83
#include <visp/vpMath.h>
84
#include <visp/vpTranslationVector.h>
85
#include <visp/vpRxyzVector.h>
86
#include <visp/vpRotationMatrix.h>
87
#include <visp/vpHomogeneousMatrix.h>
88
#include <visp/vpFeaturePoint.h>
89
#include <visp/vpPoint.h>
90
#include <visp/vpServo.h>
91
#include <visp/vpFeatureBuilder.h>
92
#include <visp/vpDot.h>
93
#include <visp/vpRobotAfma6.h>
94
#include <visp/vpServoDisplay.h>
95
#include <visp/vpPose.h>
96
#include <visp/vpIoTools.h>
97
98
// Exception
99
#include <visp/vpException.h>
100
#include <visp/vpMatrixException.h>
101
102
#define L 0.05 // to deal with a 10cm by 10cm square
103
104
130
void
compute_pose(
vpPoint
point[],
vpDot2
dot[],
int
ndot,
131
vpCameraParameters
cam,
132
vpHomogeneousMatrix
&cMo,
133
vpTranslationVector
&cto,
134
vpRxyzVector
&cro,
bool
init)
135
{
136
vpHomogeneousMatrix
cMo_dementhon;
// computed pose with dementhon
137
vpHomogeneousMatrix
cMo_lagrange;
// computed pose with dementhon
138
vpRotationMatrix
cRo;
139
vpPose
pose;
140
vpImagePoint
cog;
141
for
(
int
i=0; i < ndot; i ++) {
142
143
double
x=0, y=0;
144
145
cog = dot[i].
getCog
();
146
vpPixelMeterConversion::convertPoint
(cam, cog, x, y) ;
//pixel to meter conversion
147
// std::cout << "point cam: " << i << x << " " << y << std::endl;
148
point[i].
set_x
(x) ;
//projection perspective p
149
point[i].
set_y
(y) ;
150
pose.
addPoint
(point[i]) ;
151
// std::cout << "point " << i << std::endl;
152
// point[i].print();
153
154
}
155
156
if
(init ==
true
) {
157
pose.
computePose
(
vpPose::DEMENTHON
, cMo_dementhon) ;
158
//compute the pose for a given method
159
// cMo_dementhon.extract(cto);
160
// cMo_dementhon.extract(cRo);
161
// cro.buildFrom(cRo);
162
// Compute and return the residual expressed in meter for the pose matrix
163
// 'cMo'
164
double
residual_dementhon = pose.
computeResidual
(cMo_dementhon);
165
166
// std::cout << "\nPose Dementhon "
167
// << "(residual: " << residual_dementhon << ")\n "
168
// << "cdto[0] = " << cto[0] << ";\n "
169
// << "cdto[1] = " << cto[1] << ";\n "
170
// << "cdto[2] = " << cto[2] << ";\n "
171
// << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
172
// << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
173
// << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
174
// << std::endl;
175
176
pose.
computePose
(
vpPose::LAGRANGE
, cMo_lagrange) ;
177
// cMo_lagrange.extract(cto);
178
// cMo_lagrange.extract(cRo);
179
// cro.buildFrom(cRo);
180
double
residual_lagrange = pose.
computeResidual
(cMo_lagrange);
181
182
// std::cout << "\nPose Lagrange "
183
// << "(residual: " << residual_lagrange << ")\n "
184
// << "cdto[0] = " << cto[0] << ";\n "
185
// << "cdto[1] = " << cto[1] << ";\n "
186
// << "cdto[2] = " << cto[2] << ";\n "
187
// << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
188
// << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
189
// << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
190
// << std::endl;
191
192
// cout << "Lagrange residual term: " << residual_lagrange <<endl ;
193
194
// Select the best pose to initialize the lowe pose computation
195
if
(residual_lagrange < residual_dementhon)
//on garde le cMo
196
cMo = cMo_lagrange;
197
else
198
cMo = cMo_dementhon;
199
200
// cout <<"------------------------------------------------------------"<<endl
201
}
202
else
{
// init = false; use of the previous pose to initialise LOWE
203
cRo.
buildFrom
(cro);
204
cMo.
buildFrom
(cto, cRo);
205
}
206
pose.
computePose
(
vpPose::LOWE
, cMo) ;
207
cMo.
extract
(cto);
208
cMo.
extract
(cRo);
209
cro.
buildFrom
(cRo);
210
// double residual_lowe = pose.computeResidual(cMo);
211
212
// std::cout << "\nPose LOWE "
213
// << "(residual: " << residual_lowe << ")\n "
214
// << "cdto[0] = " << cto[0] << ";\n "
215
// << "cdto[1] = " << cto[1] << ";\n "
216
// << "cdto[2] = " << cto[2] << ";\n "
217
// << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
218
// << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
219
// << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
220
// << std::endl;
221
222
// vpTRACE( "LOWE pose :" ) ;
223
// std::cout << cMo << std::endl ;
224
}
225
226
int
227
main()
228
{
229
// Log file creation in /tmp/$USERNAME/log.dat
230
// This file contains by line:
231
// - the 6 computed camera velocities (m/s, rad/s) to achieve the task
232
// - the 6 mesured joint velocities (m/s, rad/s)
233
// - the 6 mesured joint positions (m, rad)
234
// - the 8 values of s - s*
235
// - the 6 values of the pose cMo (tx,ty,tz, rx,ry,rz) with translation
236
// in meters and rotations in radians
237
std::string username;
238
// Get the user login name
239
vpIoTools::getUserName
(username);
240
241
// Create a log filename to save velocities...
242
std::string logdirname;
243
logdirname =
"/tmp/"
+ username;
244
245
// Test if the output path exist. If no try to create it
246
if
(
vpIoTools::checkDirectory
(logdirname) ==
false
) {
247
try
{
248
// Create the dirname
249
vpIoTools::makeDirectory
(logdirname);
250
}
251
catch
(...) {
252
std::cerr << std::endl
253
<<
"ERROR:"
<< std::endl;
254
std::cerr <<
" Cannot create "
<< logdirname << std::endl;
255
exit(-1);
256
}
257
}
258
std::string logfilename;
259
logfilename = logdirname +
"/log.dat"
;
260
261
// Open the log file name
262
std::ofstream flog(logfilename.c_str());
263
264
try
265
{
266
vpServo
task ;
267
268
vpImage<unsigned char>
I ;
269
int
i ;
270
271
vp1394TwoGrabber
g;
272
g.
setVideoMode
(
vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8
);
273
g.
setFramerate
(
vp1394TwoGrabber::vpFRAMERATE_60
);
274
g.
open
(I) ;
275
276
#ifdef VISP_HAVE_X11
277
vpDisplayX
display(I,100,100,
"Current image"
) ;
278
#elif defined(VISP_HAVE_OPENCV)
279
vpDisplayOpenCV
display(I,100,100,
"Current image"
) ;
280
#elif defined(VISP_HAVE_GTK)
281
vpDisplayGTK
display(I,100,100,
"Current image"
) ;
282
#endif
283
284
g.
acquire
(I) ;
285
286
vpDisplay::display
(I) ;
287
vpDisplay::flush
(I) ;
288
289
std::cout << std::endl ;
290
std::cout <<
"-------------------------------------------------------"
<< std::endl ;
291
std::cout <<
" Test program for vpServo "
<<std::endl ;
292
std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame"
<< std::endl ;
293
std::cout <<
" Use of the Afma6 robot "
<< std::endl ;
294
std::cout <<
" Interaction matrix computed with the current features "
<< std::endl ;
295
std::cout <<
" task : servo 4 points on a square with dimention "
<< L <<
" meters"
<< std::endl ;
296
std::cout <<
"-------------------------------------------------------"
<< std::endl ;
297
std::cout << std::endl ;
298
299
300
vpDot2
dot[4] ;
301
vpImagePoint
cog;
302
303
std::cout <<
"Click on the 4 dots clockwise starting from upper/left dot..."
304
<< std::endl;
305
for
(i=0 ; i < 4 ; i++) {
306
dot[i].
initTracking
(I) ;
307
cog = dot[i].
getCog
();
308
vpDisplay::displayCross
(I, cog, 10,
vpColor::blue
) ;
309
vpDisplay::flush
(I);
310
}
311
312
vpCameraParameters::vpCameraParametersProjType
313
projModel =
vpCameraParameters::perspectiveProjWithDistortion
;
314
vpRobotAfma6
robot;
315
316
// Load the end-effector to camera frame transformation obtained
317
// using a camera intrinsic model with distortion
318
robot.
init
(
vpAfma6::TOOL_CCMOP
, projModel);
319
320
vpCameraParameters
cam ;
321
// Update camera parameters
322
robot.
getCameraParameters
(cam, I);
323
324
// Sets the current position of the visual feature
325
vpFeaturePoint
p[4] ;
326
for
(i=0 ; i < 4 ; i++)
327
vpFeatureBuilder::create
(p[i], cam, dot[i]);
//retrieve x,y of the vpFeaturePoint structure
328
329
// Set the position of the square target in a frame which origin is
330
// centered in the middle of the square
331
vpPoint
point[4] ;
332
point[0].
setWorldCoordinates
(-L, -L, 0) ;
333
point[1].
setWorldCoordinates
( L, -L, 0) ;
334
point[2].
setWorldCoordinates
( L, L, 0) ;
335
point[3].
setWorldCoordinates
(-L, L, 0) ;
336
337
// Initialise a desired pose to compute s*, the desired 2D point features
338
vpHomogeneousMatrix
cMo;
339
vpTranslationVector
cto(0, 0, 0.7);
// tz = 0.7 meter
340
vpRxyzVector
cro(
vpMath::rad
(0),
vpMath::rad
(0),
vpMath::rad
(0));
// No rotations
341
vpRotationMatrix
cRo(cro);
// Build the rotation matrix
342
cMo.
buildFrom
(cto, cRo);
// Build the homogeneous matrix
343
344
// Sets the desired position of the 2D visual feature
345
vpFeaturePoint
pd[4] ;
346
// Compute the desired position of the features from the desired pose
347
for
(
int
i=0; i < 4; i ++) {
348
vpColVector
cP, p ;
349
point[i].
changeFrame
(cMo, cP) ;
350
point[i].
projection
(cP, p) ;
351
352
pd[i].
set_x
(p[0]) ;
353
pd[i].
set_y
(p[1]) ;
354
pd[i].
set_Z
(cP[2]);
355
}
356
357
// Define the task
358
// - we want an eye-in-hand control law
359
// - robot is controlled in the camera frame
360
// - Interaction matrix is computed with the current visual features
361
task.
setServo
(
vpServo::EYEINHAND_CAMERA
) ;
362
task.
setInteractionMatrixType
(
vpServo::CURRENT
,
vpServo::PSEUDO_INVERSE
);
363
364
// We want to see a point on a point
365
std::cout << std::endl ;
366
for
(i=0 ; i < 4 ; i++)
367
task.
addFeature
(p[i],pd[i]) ;
368
369
// Set the proportional gain
370
task.
setLambda
(0.1) ;
371
372
// Display task information
373
task.
print
() ;
374
375
// Initialise the velocity control of the robot
376
robot.
setRobotState
(
vpRobot::STATE_VELOCITY_CONTROL
) ;
377
378
// Initialise the pose using Lagrange and Dementhon methods, chose the best
379
// estimated pose (either Lagrange or Dementhon) and than compute the pose
380
// using LOWE method with Lagrange or Dementhon pose as initialisation.
381
// compute_pose(point, dot, 4, cam, cMo, cto, cro, true);
382
383
std::cout <<
"\nHit CTRL-C to stop the loop...\n"
<< std::flush;
384
385
for
( ; ; ) {
386
// Acquire a new image from the camera
387
g.
acquire
(I) ;
388
389
// Display this image
390
vpDisplay::display
(I) ;
391
392
// For each point...
393
for
(i=0 ; i < 4 ; i++) {
394
// Achieve the tracking of the dot in the image
395
dot[i].
track
(I) ;
396
// Get the dot cog
397
cog = dot[i].
getCog
();
398
// Display a green cross at the center of gravity position in the
399
// image
400
vpDisplay::displayCross
(I, cog, 10,
vpColor::green
) ;
401
}
402
403
// During the servo, we compute the pose using LOWE method. For the
404
// initial pose used in the non linear minimisation we use the pose
405
// computed at the previous iteration.
406
compute_pose(point, dot, 4, cam, cMo, cto, cro,
false
);
407
408
for
(i=0 ; i < 4 ; i++) {
409
// Update the point feature from the dot location
410
vpFeatureBuilder::create
(p[i], cam, dot[i]);
411
// Set the feature Z coordinate from the pose
412
vpColVector
cP;
413
point[i].
changeFrame
(cMo, cP) ;
414
415
p[i].set_Z(cP[2]);
416
}
417
418
// Printing on stdout concerning task information
419
// task.print() ;
420
421
vpColVector
v ;
422
// Compute the visual servoing skew vector
423
v = task.
computeControlLaw
() ;
424
425
// Display the current and desired feature points in the image display
426
vpServoDisplay::display
(task, cam, I);
427
428
// Apply the computed camera velocities to the robot
429
robot.
setVelocity
(
vpRobot::CAMERA_FRAME
, v) ;
430
431
// Save velocities applied to the robot in the log file
432
// v[0], v[1], v[2] correspond to camera translation velocities in m/s
433
// v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
434
flog << v[0] <<
" "
<< v[1] <<
" "
<< v[2] <<
" "
435
<< v[3] <<
" "
<< v[4] <<
" "
<< v[5] <<
" "
;
436
437
// Get the measured joint velocities of the robot
438
vpColVector
qvel;
439
robot.
getVelocity
(
vpRobot::ARTICULAR_FRAME
, qvel);
440
// Save measured joint velocities of the robot in the log file:
441
// - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
442
// velocities in m/s
443
// - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
444
// velocities in rad/s
445
flog << qvel[0] <<
" "
<< qvel[1] <<
" "
<< qvel[2] <<
" "
446
<< qvel[3] <<
" "
<< qvel[4] <<
" "
<< qvel[5] <<
" "
;
447
448
// Get the measured joint positions of the robot
449
vpColVector
q;
450
robot.
getPosition
(
vpRobot::ARTICULAR_FRAME
, q);
451
// Save measured joint positions of the robot in the log file
452
// - q[0], q[1], q[2] correspond to measured joint translation
453
// positions in m
454
// - q[3], q[4], q[5] correspond to measured joint rotation
455
// positions in rad
456
flog << q[0] <<
" "
<< q[1] <<
" "
<< q[2] <<
" "
457
<< q[3] <<
" "
<< q[4] <<
" "
<< q[5] <<
" "
;
458
459
// Save feature error (s-s*) for the 4 feature points. For each feature
460
// point, we have 2 errors (along x and y axis). This error is expressed
461
// in meters in the camera frame
462
flog << ( task.
getError
() ).t()<<
" "
;
// s-s* for points
463
464
// Save the current cMo pose: translations in meters, rotations (rx, ry,
465
// rz) in radians
466
flog << cto[0] <<
" "
<< cto[1] <<
" "
<< cto[2] <<
" "
// translation
467
<< cro[0] <<
" "
<< cro[1] <<
" "
<< cro[2] << std::endl;
// rot
468
469
// Flush the display
470
vpDisplay::flush
(I) ;
471
}
472
473
flog.close() ;
// Close the log file
474
475
// Display task information
476
task.
print
() ;
477
478
// Kill the task
479
task.
kill
();
480
481
return
0;
482
}
483
catch
(...) {
484
flog.close() ;
// Close the log file
485
486
vpERROR_TRACE
(
" Test failed"
) ;
487
return
0;
488
}
489
}
490
491
#else
492
int
493
main()
494
{
495
vpERROR_TRACE
(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer..."
);
496
497
}
498
499
#endif
example
servo-afma6
servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp
Generated on Fri Sep 27 2013 21:09:13 for ViSP by
1.8.4