53 #include <visp/vpConfig.h>
54 #include <visp/vpDebug.h>
57 #ifdef VISP_HAVE_COIN_AND_GUI
59 #include <visp/vpImage.h>
60 #include <visp/vpCameraParameters.h>
61 #include <visp/vpTime.h>
62 #include <visp/vpSimulator.h>
65 #include <visp/vpMath.h>
66 #include <visp/vpHomogeneousMatrix.h>
67 #include <visp/vpFeaturePointPolar.h>
68 #include <visp/vpServo.h>
69 #include <visp/vpRobotCamera.h>
70 #include <visp/vpFeatureBuilder.h>
71 #include <visp/vpParseArgv.h>
72 #include <visp/vpIoTools.h>
74 #define GETOPTARGS "di:h"
86 void usage(
const char *name,
const char *badparam, std::string ipath)
89 Simulation Servo 4points.\n\
92 %s [-i <input image path>] [-d] [-h]\n", name);
96 -i <input image path> %s\n\
97 Set image input path.\n\
98 From this path read \"ViSP-images/iv/4points.iv\"\n\
100 Setting the VISP_INPUT_IMAGE_PATH environment\n\
101 variable produces the same behaviour than using\n\
105 Disable the image display. This can be useful \n\
106 for automatic tests using crontab under Unix or \n\
107 using the task manager under Windows.\n\
110 Print the help.\n\n", ipath.c_str());
113 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
131 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &display)
138 case 'i': ipath = optarg;
break;
139 case 'd': display =
false;
break;
140 case 'h': usage(argv[0], NULL, ipath);
return false;
break;
143 usage(argv[0], optarg, ipath);
return false;
break;
147 if ((c == 1) || (c == -1)) {
149 usage(argv[0], NULL, ipath);
150 std::cerr <<
"ERROR: " << std::endl;
151 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
159 void *mainLoop (
void *_simu)
168 float sampling_time = 0.040f;
171 std::cout << std::endl ;
172 std::cout <<
"-------------------------------------------------------" << std::endl ;
173 std::cout <<
" Test program for vpServo " <<std::endl ;
174 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl ;
175 std::cout <<
" Simulation " << std::endl ;
176 std::cout <<
" task : servo 4 points " << std::endl ;
177 std::cout <<
"-------------------------------------------------------" << std::endl ;
178 std::cout << std::endl ;
181 vpTRACE(
"sets the initial camera location " ) ;
200 vpTRACE(
"sets the point coordinates in the world frame " ) ;
207 vpTRACE(
"project : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
208 for (
int i = 0 ; i < 4 ; i++) {
213 vpTRACE(
"sets the desired position of the point ") ;
215 for (
int i = 0 ; i < 4 ; i++)
218 std::cout <<
"s: \n";
219 for (
int i=0; i < 4; i ++) {
220 printf(
"[%d] rho %f theta %f Z %f\n",
221 i, p[i].get_rho(), p[i].get_theta(), p[i].get_Z());
224 vpTRACE(
"sets the desired position of the point ") ;
240 for (
int i=0; i < 4; i ++) {
246 std::cout <<
"s*: \n";
247 for (
int i=0; i < 4; i ++) {
248 printf(
"[%d] rho %f theta %f Z %f\n",
249 i, pd[i].get_rho(), pd[i].get_theta(), pd[i].get_Z());
253 vpTRACE(
"\t we want an eye-in-hand control law") ;
254 vpTRACE(
"\t articular velocity are computed") ;
258 vpTRACE(
"Set the position of the camera in the end-effector frame ") ;
263 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
268 vpTRACE(
"\t we want to see a point on a point..") ;
269 for (
int i = 0 ; i < 4 ; i++)
276 vpTRACE(
"Display task information " ) ;
280 std::cout <<
"\nEnter a character to continue or CTRL-C to quit... "
282 {
char a ; std::cin >> a ; }
285 char name[FILENAME_MAX];
286 unsigned int iter=0 ;
288 while(iter++ < 300) {
297 for (
int i = 0 ; i < 4 ; i++)
299 point[i].
track(cMo) ;
314 sprintf(name,
"/tmp/image.%04d.external.png",iter) ;
315 std::cout << name << std::endl ;
317 sprintf(name,
"/tmp/image.%04d.internal.png",iter) ;
324 vpTRACE(
"Display task information " ) ;
328 std::cout <<
"cMo:\n" << cMo << std::endl;
330 std::cout <<
"final pose:\n" << pose.t() << std::endl;
332 std::cout <<
"\nEnter a character to continue..." <<std::endl ;
333 {
char a ; std::cin >> a ; }
346 main(
int argc,
const char ** argv)
348 std::string env_ipath;
349 std::string opt_ipath;
351 std::string filename;
352 std::string username;
353 bool opt_display =
true;
356 char *ptenv = getenv(
"VISP_INPUT_IMAGE_PATH");
361 if (! env_ipath.empty())
365 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
370 if (!opt_ipath.empty())
375 if (!opt_ipath.empty() && !env_ipath.empty()) {
376 if (ipath != env_ipath) {
377 std::cout << std::endl
378 <<
"WARNING: " << std::endl;
379 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
380 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
381 <<
" we skip the environment variable." << std::endl;
386 if (opt_ipath.empty() && env_ipath.empty()){
387 usage(argv[0], NULL, ipath);
388 std::cerr << std::endl
389 <<
"ERROR:" << std::endl;
390 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
392 <<
" environment variable to specify the location of the " << std::endl
393 <<
" image path where test images are located." << std::endl << std::endl;
411 simu.
load(filename.c_str()) ;
424 {
vpTRACE(
"You should install Coin3D and SoQT or SoWin or SoXt") ;