59 #include <visp/vpConfig.h>
60 #include <visp/vpDebug.h>
64 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
66 #include <visp/vp1394TwoGrabber.h>
67 #include <visp/vpImage.h>
68 #include <visp/vpImageIo.h>
69 #include <visp/vpDisplay.h>
70 #include <visp/vpDisplayX.h>
71 #include <visp/vpDisplayOpenCV.h>
72 #include <visp/vpDisplayGTK.h>
74 #include <visp/vpMath.h>
75 #include <visp/vpHomogeneousMatrix.h>
76 #include <visp/vpFeatureLine.h>
77 #include <visp/vpMeLine.h>
78 #include <visp/vpCylinder.h>
79 #include <visp/vpServo.h>
80 #include <visp/vpFeatureBuilder.h>
82 #include <visp/vpRobotAfma6.h>
85 #include <visp/vpException.h>
86 #include <visp/vpMatrixException.h>
87 #include <visp/vpServoDisplay.h>
104 vpDisplayX display(I,100,100,
"Current image") ;
105 #elif defined(VISP_HAVE_OPENCV)
107 #elif defined(VISP_HAVE_GTK)
115 std::cout << std::endl ;
116 std::cout <<
"-------------------------------------------------------" << std::endl ;
117 std::cout <<
" Test program for vpServo " <<std::endl ;
118 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
119 std::cout <<
" Simulation " << std::endl ;
120 std::cout <<
" task : servo a point " << std::endl ;
121 std::cout <<
"-------------------------------------------------------" << std::endl ;
122 std::cout << std::endl ;
136 for (i=0 ; i < nbline ; i++)
152 vpTRACE(
"sets the current position of the visual feature ") ;
154 for (i=0 ; i < nbline ; i++)
157 vpTRACE(
"sets the desired position of the visual feature ") ;
174 vpTRACE(
"\t we want an eye-in-hand control law") ;
175 vpTRACE(
"\t robot is controlled in the camera frame") ;
179 vpTRACE(
"\t we want to see a two lines on two lines..") ;
180 std::cout << std::endl ;
181 for (i=0 ; i < nbline ; i++)
188 vpTRACE(
"Display task information " ) ;
194 unsigned int iter=0 ;
198 double lambda_av =0.05;
203 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
210 for (i=0 ; i < nbline ; i++)
216 vpTRACE(
"%f %f ",line[i].getRho(), line[i].getTheta()) ;
227 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
231 gain = alpha * exp (-beta * ( task.
getError() ).sumSquare() ) + lambda_av ;
253 vpTRACE(
"Display task information " ) ;
268 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");