88 #include <visp/vpMath.h>
89 #include <visp/vpFeatureThetaU.h>
90 #include <visp/vpFeatureTranslation.h>
91 #include <visp/vpHomogeneousMatrix.h>
92 #include <visp/vpIoTools.h>
93 #include <visp/vpParseArgv.h>
94 #include <visp/vpServo.h>
95 #include <visp/vpSimulatorCamera.h>
98 #define GETOPTARGS "h"
108 void usage(
const char *name,
const char *badparam)
111 Simulation of a 3D visual servoing:\n\
112 - eye-in-hand control law,\n\
113 - velocity computed in the camera frame,\n\
114 - without display.\n\
126 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
138 bool getOptions(
int argc,
const char **argv)
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)
168 if (getOptions(argc, argv) ==
false) {
176 std::string username;
181 std::string logdirname;
183 logdirname =
"C:/temp/" + username;
185 logdirname =
"/tmp/" + username;
194 std::cerr << std::endl
195 <<
"ERROR:" << std::endl;
196 std::cerr <<
" Cannot create " << logdirname << std::endl;
200 std::string logfilename;
201 logfilename = logdirname +
"/log.dat";
204 std::ofstream flog(logfilename.c_str());
208 std::cout << std::endl ;
209 std::cout <<
"-------------------------------------------------------" << std::endl ;
210 std::cout <<
" Test program for vpServo " <<std::endl ;
211 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
212 std::cout <<
" Simulation " << std::endl ;
213 std::cout <<
" task : 3D visual servoing " << std::endl ;
214 std::cout <<
"-------------------------------------------------------" << std::endl ;
215 std::cout << std::endl ;
228 robot.getPosition(wMc) ;
246 unsigned int iter=0 ;
248 while(iter++ < 200) {
249 std::cout <<
"------------------------------------" << iter <<std::endl ;
252 robot.getPosition(wMc) ;
275 w = lambda * tu_cRcd;
279 for (
unsigned int i=0; i<3; i++) {
281 velocity[i+3] = w[i];
288 std::cout <<
"|| s - s* || = " << ctcd.
t() <<
" " << tu_cRcd.
t() << std::endl;
291 flog << velocity.
t() <<
" " << ctcd.
t() <<
" " << tu_cRcd.
t() << std::endl;