60 #include <visp/vpDebug.h>
61 #include <visp/vpConfig.h>
63 #if (defined(WIN32) || defined(VISP_HAVE_PTHREAD)) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI))
72 #include <visp/vpCameraParameters.h>
73 #include <visp/vpDisplayX.h>
74 #include <visp/vpDisplayGTK.h>
75 #include <visp/vpDisplayGDI.h>
76 #include <visp/vpFeatureBuilder.h>
77 #include <visp/vpFeaturePoint.h>
78 #include <visp/vpHomogeneousMatrix.h>
79 #include <visp/vpImage.h>
80 #include <visp/vpImagePoint.h>
81 #include <visp/vpIoTools.h>
82 #include <visp/vpMath.h>
83 #include <visp/vpMeterPixelConversion.h>
84 #include <visp/vpParseArgv.h>
85 #include <visp/vpServo.h>
86 #include <visp/vpSimulatorViper850.h>
89 #define GETOPTARGS "cdh"
99 void usage(
const char *name,
const char *badparam)
102 Tests a control law with the following characteristics:\n\
103 - eye-in-hand control\n\
104 - articular velocity are computed\n\
105 - servo on 4 points,\n\
106 - internal and external camera view displays.\n\
109 %s [-c] [-d] [-h]\n", name);
114 Disable the mouse click. Useful to automaze the \n\
115 execution of this program without humain intervention.\n\
118 Turn off the display.\n\
124 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
138 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
145 case 'c': click_allowed =
false;
break;
146 case 'd': display =
false;
break;
147 case 'h': usage(argv[0], NULL);
return false;
break;
150 usage(argv[0], optarg);
155 if ((c == 1) || (c == -1)) {
157 usage(argv[0], NULL);
158 std::cerr <<
"ERROR: " << std::endl;
159 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
167 main(
int argc,
const char ** argv)
169 bool opt_click_allowed =
true;
170 bool opt_display =
true;
173 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
179 #if defined VISP_HAVE_X11
181 #elif defined VISP_HAVE_GDI
183 #elif defined VISP_HAVE_OPENCV
192 displayInt.
init(Iint,700,0,
"Internal view") ;
198 std::cout << std::endl ;
199 std::cout <<
"----------------------------------------------" << std::endl ;
200 std::cout <<
" Test program for vpServo " <<std::endl ;
201 std::cout <<
" Eye-in-hand task control, articular velocity are computed"
203 std::cout <<
" Simulation " << std::endl ;
204 std::cout <<
" task : servo 4 points " << std::endl ;
205 std::cout <<
"----------------------------------------------" << std::endl ;
206 std::cout << std::endl ;
221 for (i = 0 ; i < 4 ; i++)
222 point[i].track(cMo) ;
226 for (i = 0 ; i < 4 ; i++)
236 for (
int i = 0 ; i < 4 ; i++)
237 point[i].track(cdMo);
239 for (
int i = 0 ; i < 4 ; i++)
249 for (i = 0 ; i < 4 ; i++)
266 robot.initialiseObjectRelativeToCamera(cMo);
269 robot.setDesiredCameraPosition(cdMo);
273 robot.getCameraParameters(cam,Iint);
279 robot.getInternalView(Iint);
286 unsigned int iter=0 ;
290 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
297 cMo = robot.get_cMo();
300 std::cout <<
"Initial robot position with respect to the object frame:\n";
305 for (i = 0 ; i < 4 ; i++)
307 point[i].
track(cMo) ;
322 robot.getInternalView(Iint);
326 if (opt_display && opt_click_allowed && iter == 1)
329 std::cout <<
"Click in the internal view window to continue..." << std::endl;
339 std::cout <<
"|| s - s* || " << ( task.
getError() ).sumSquare() <<std::endl ;
349 std::cout <<
"Final robot position with respect to the object frame:\n";
352 if (opt_display && opt_click_allowed)
355 std::cout <<
"Click in the internal view window to end..." << std::endl;
363 vpERROR_TRACE(
"You do not have X11, OpenCV or GDI display functionalities or threading capabilities...");