60 #include <visp/vpDebug.h>
61 #include <visp/vpConfig.h>
63 #if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
68 #include <visp/vpCameraParameters.h>
69 #include <visp/vpDisplayX.h>
70 #include <visp/vpDisplayGTK.h>
71 #include <visp/vpDisplayGDI.h>
72 #include <visp/vpFeatureBuilder.h>
73 #include <visp/vpFeaturePointPolar.h>
74 #include <visp/vpHomogeneousMatrix.h>
75 #include <visp/vpImage.h>
76 #include <visp/vpImagePoint.h>
77 #include <visp/vpIoTools.h>
78 #include <visp/vpMath.h>
79 #include <visp/vpMeterPixelConversion.h>
80 #include <visp/vpProjectionDisplay.h>
81 #include <visp/vpServo.h>
82 #include <visp/vpServoDisplay.h>
83 #include <visp/vpSimulatorCamera.h>
84 #include <visp/vpParseArgv.h>
87 #define GETOPTARGS "cdh"
97 void usage(
const char *name,
const char *badparam)
100 Tests a control law with the following characteristics:\n\
101 - eye-in-hand control\n\
102 - articular velocity are computed\n\
103 - servo on 4 points,\n\
104 - internal and external camera view displays.\n\
107 %s [-c] [-d] [-h]\n", name);
112 Disable the mouse click. Useful to automaze the \n\
113 execution of this program without humain intervention.\n\
116 Turn off the display.\n\
122 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
136 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
143 case 'c': click_allowed =
false;
break;
144 case 'd': display =
false;
break;
145 case 'h': usage(argv[0], NULL);
return false;
break;
148 usage(argv[0], optarg);
153 if ((c == 1) || (c == -1)) {
155 usage(argv[0], NULL);
156 std::cerr <<
"ERROR: " << std::endl;
157 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
165 main(
int argc,
const char ** argv)
174 std::string username;
179 std::string logdirname;
181 logdirname =
"C:/temp/" + username;
183 logdirname =
"/tmp/" + username;
193 std::cerr << std::endl
194 <<
"ERROR:" << std::endl;
195 std::cerr <<
" Cannot create " << logdirname << std::endl;
199 std::string logfilename;
200 logfilename = logdirname +
"/log.dat";
203 std::ofstream flog(logfilename.c_str());
206 bool opt_click_allowed =
true;
207 bool opt_display =
true;
210 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
216 #if defined VISP_HAVE_X11
219 #elif defined VISP_HAVE_GTK
222 #elif defined VISP_HAVE_GDI
233 displayInt.
init(Iint,0,0,
"Internal view") ;
234 displayExt.
init(Iext,330,000,
"External view") ;
239 double px, py ; px = py = 500 ;
240 double u0, v0 ; u0 = 150, v0 = 160 ;
249 std::cout << std::endl ;
250 std::cout <<
"----------------------------------------------" << std::endl ;
251 std::cout <<
" Test program for vpServo " <<std::endl ;
252 std::cout <<
" Eye-in-hand task control, articular velocity are computed"
254 std::cout <<
" Simulation " << std::endl ;
255 std::cout <<
" task : servo 4 points " << std::endl ;
256 std::cout <<
"----------------------------------------------" << std::endl ;
257 std::cout << std::endl ;
266 #if defined(TRANS_Z_PURE)
273 #elif defined(TRANS_X_PURE)
281 #elif defined(ROT_Z_PURE)
289 #elif defined(ROT_X_PURE)
297 #elif defined(COMPLEX)
305 #elif defined(PROBLEM)
332 for (i = 0 ; i < 4 ; i++)
333 externalview.
insert(point[i]) ;
340 for (i = 0 ; i < 4 ; i++) {
341 point[i].
track(cMod);
350 for (i = 0 ; i < 4 ; i++)
351 point[i].track(cMo) ;
355 for (i = 0 ; i < 4 ; i++) {
381 for (i = 0 ; i < 4 ; i++)
388 std::cout <<
"\nDisplay task information: " << std::endl;
391 unsigned int iter=0 ;
393 while(iter++ < 200) {
394 std::cout <<
"---------------------------------------------"
395 << iter <<std::endl ;
410 for (i = 0 ; i < 4 ; i++) {
411 point[i].
track(cMo) ;
430 std::cout <<
"Display task information: " << std::endl;
442 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" "
443 << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
445 std::cout <<
"v: " << v.
t() << std::endl;
447 std::cout <<
"|| s - s* || = "<< ( task.
getError() ).sumSquare() << std::endl;
452 flog << ( task.
getError() ).t() <<
" ";
453 std::cout <<
"|| s - s* || = " << ( task.
getError() ).sumSquare() <<std::endl ;
456 for (i = 0 ; i < 4 ; i++) {
460 for (i = 0 ; i < 4 ; i++) {
461 flog << point[i].
get_x() <<
" " << point[i].
get_y() <<
" ";
470 std::cout <<
"\nClick in the internal camera view to continue..." << std::endl;
488 std::cout <<
"Final robot position with respect to the object frame:\n";
491 if (opt_display && opt_click_allowed) {
493 std::cout <<
"\n\nClick in the internal view to end..." << std::endl;
501 vpERROR_TRACE(
"You do not have X11, GTK or GDI display functionalities...");