64 #include <visp/vpConfig.h>
65 #include <visp/vpDebug.h>
70 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
74 #include <visp/vp1394TwoGrabber.h>
75 #include <visp/vpImage.h>
76 #include <visp/vpImagePoint.h>
77 #include <visp/vpMath.h>
78 #include <visp/vpHomogeneousMatrix.h>
79 #include <visp/vpFeaturePoint.h>
80 #include <visp/vpPoint.h>
81 #include <visp/vpServo.h>
82 #include <visp/vpFeatureBuilder.h>
83 #include <visp/vpRobotAfma6.h>
84 #include <visp/vpException.h>
85 #include <visp/vpMatrixException.h>
86 #include <visp/vpServoDisplay.h>
87 #include <visp/vpDot.h>
88 #include <visp/vpPose.h>
89 #include <visp/vpImageIo.h>
90 #include <visp/vpDisplay.h>
91 #include <visp/vpDisplayX.h>
92 #include <visp/vpDisplayOpenCV.h>
93 #include <visp/vpDisplayGTK.h>
116 vpDisplayX display(I,100,100,
"Current image") ;
117 #elif defined(VISP_HAVE_OPENCV)
119 #elif defined(VISP_HAVE_GTK)
126 std::cout << std::endl ;
127 std::cout <<
"-------------------------------------------------------" << std::endl ;
128 std::cout <<
" Test program for vpServo " <<std::endl ;
129 std::cout <<
" Eye-to-hand task control" << std::endl ;
130 std::cout <<
" Simulation " << std::endl ;
131 std::cout <<
" task : servo a point " << std::endl ;
132 std::cout <<
"-------------------------------------------------------" << std::endl ;
133 std::cout << std::endl ;
140 for (i=0 ; i < nbPoint ; i++)
167 for (i=0 ; i < nbPoint ; i++)
182 std::cout << cMo << std::endl ;
190 std::cout <<
" Learning 0/1 " <<std::endl ;
191 char name[FILENAME_MAX] ;
192 sprintf(name,
"cdMo.dat") ;
194 std::cin >> learning ;
198 vpTRACE(
"Save the location of the object in a file cdMo.dat") ;
199 std::ofstream f(name) ;
207 vpTRACE(
"Loading desired location from cdMo.dat") ;
208 std::ifstream f(
"cdMo.dat") ;
217 for (i=0 ; i < nbPoint ; i++)
232 vpTRACE(
"\t we want an eye-in-hand control law") ;
233 vpTRACE(
"\t robot is controlled in the camera frame") ;
238 for (i=0 ; i < nbPoint ; i++)
244 vpTRACE(
"Display task information " ) ;
251 double convergence_threshold = 0.00;
256 unsigned int iter=0 ;
264 oMcamrobot[0][3] = -0.05 ;
269 double lambda_av =0.1;
273 std::cout <<
"alpha 0.7" << std::endl;
275 std::cout <<
"beta 5" << std::endl;
277 std::list<vpImagePoint> Lcog ;
279 while(error > convergence_threshold)
281 std::cout <<
"---------------------------------------------" << iter++ <<std::endl ;
288 "Eye-To-Hand Visual Servoing",
293 "IRISA-INRIA Rennes, Lagadic project",
297 for (i=0 ; i < nbPoint ; i++)
300 Lcog.push_back( dot[i].getCog() );
305 vpTRACE(
"Error detected while tracking visual features") ;
313 for (i=0 ; i < nbPoint ; i++)
339 cMe = cMo *oMcamrobot * camrobotMe ;
353 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
357 gain = alpha * exp (-beta * ( task.
getError() ).sumSquare() ) + lambda_av;
360 else gain = lambda_av ;
364 vpTRACE(
"%f %f %f %f %f",alpha, beta, lambda_av, ( task.
getError() ).sumSquare(), gain) ;
371 for (std::list<vpImagePoint>::const_iterator it_cog = Lcog.begin(); it_cog != Lcog.end(); ++it_cog)
378 error = ( task.
getError() ).sumSquare() ;
379 std::cout <<
"|| s - s* || = "<< error<<std::endl ;
383 vpTRACE(
"Error detected while tracking visual features") ;
392 if ((SAVE==1) && (iter %3==0))
396 sprintf(name,
"/tmp/marchand/image.%04d.ppm",it++) ;
416 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");