63 #include <visp/vpConfig.h>
64 #include <visp/vpDebug.h>
68 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
70 #include <visp/vp1394TwoGrabber.h>
71 #include <visp/vpImage.h>
72 #include <visp/vpImageIo.h>
73 #include <visp/vpDisplay.h>
74 #include <visp/vpDisplayX.h>
75 #include <visp/vpDisplayOpenCV.h>
76 #include <visp/vpDisplayGTK.h>
78 #include <visp/vpMath.h>
79 #include <visp/vpHomogeneousMatrix.h>
80 #include <visp/vpFeatureLine.h>
81 #include <visp/vpMeLine.h>
82 #include <visp/vpCylinder.h>
83 #include <visp/vpServo.h>
84 #include <visp/vpFeatureBuilder.h>
86 #include <visp/vpRobotAfma6.h>
89 #include <visp/vpException.h>
90 #include <visp/vpMatrixException.h>
91 #include <visp/vpServoDisplay.h>
109 vpDisplayX display(I,100,100,
"Current image") ;
110 #elif defined(VISP_HAVE_OPENCV)
112 #elif defined(VISP_HAVE_GTK)
121 std::cout << std::endl ;
122 std::cout <<
"-------------------------------------------------------" << std::endl ;
123 std::cout <<
" Test program for vpServo " <<std::endl ;
124 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
125 std::cout <<
" Simulation " << std::endl ;
126 std::cout <<
" task : servo a point " << std::endl ;
127 std::cout <<
"-------------------------------------------------------" << std::endl ;
128 std::cout << std::endl ;
142 for (i=0 ; i < nbline ; i++)
158 vpTRACE(
"sets the current position of the visual feature ") ;
160 for (i=0 ; i < nbline ; i++)
163 vpTRACE(
"sets the desired position of the visual feature ") ;
180 vpTRACE(
"\t we want an eye-in-hand control law") ;
181 vpTRACE(
"\t robot is controlled in the camera frame") ;
185 vpTRACE(
"\t we want to see a point on a point..") ;
186 std::cout << std::endl ;
187 for (i=0 ; i < nbline ; i++)
194 vpTRACE(
"Display task information " ) ;
200 unsigned int iter=0 ;
204 double lambda_av =0.05;
211 while(erreur > 0.00001)
213 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
220 for (i=0 ; i < nbline ; i++)
236 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
240 gain = alpha * exp (-beta * ( task.
getError() ).sumSquare() ) + lambda_av ;
258 erreur = ( task.
getError() ).sumSquare();
272 double vitesse = 0.02;
273 unsigned int tempo = 1200;
277 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
284 for (i=0 ; i < nbline ; i++)
300 if ( iter%tempo < 400 /*&& iter%tempo >= 0*/)
303 e1[0] = fabs(vitesse) ;
305 rapport = vitesse/proj_e1[0];
308 if ( iter == 199 ) iter+=200;
311 if ( iter%tempo < 600 && iter%tempo >= 400)
314 e2[1] = fabs(vitesse) ;
316 rapport = vitesse/proj_e2[1];
321 if ( iter%tempo < 1000 && iter%tempo >= 600)
324 e1[0] = -fabs(vitesse) ;
326 rapport = -vitesse/proj_e1[0];
331 if ( iter%tempo < 1200 && iter%tempo >= 1000)
334 e2[1] = -fabs(vitesse) ;
336 rapport = -vitesse/proj_e2[1];
356 vpTRACE(
"Display task information " ) ;
371 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");