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/vpLine.h>
82 #include <visp/vpMeLine.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>
93 #include <visp/vpDot.h>
110 vpDisplayX display(I,100,100,
"Current image") ;
111 #elif defined(VISP_HAVE_OPENCV)
113 #elif defined(VISP_HAVE_GTK)
123 std::cout << std::endl ;
124 std::cout <<
"-------------------------------------------------------" << std::endl ;
125 std::cout <<
" Test program for vpServo " <<std::endl ;
126 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
127 std::cout <<
" Simulation " << std::endl ;
128 std::cout <<
" task : servo a line " << std::endl ;
129 std::cout <<
"-------------------------------------------------------" << std::endl ;
130 std::cout << std::endl ;
144 for (i=0 ; i < nbline ; i++)
160 vpTRACE(
"sets the current position of the visual feature ") ;
162 for (i=0 ; i < nbline ; i++)
165 vpTRACE(
"sets the desired position of the visual feature ") ;
181 lined[0].
setRho(-fabs(lined[0].getRho()));
183 lined[1].
setRho(-fabs(lined[1].getRho()));
185 lined[2].
setRho(-fabs(lined[2].getRho()));
187 lined[3].
setRho(-fabs(lined[3].getRho()));
198 vpTRACE(
"\t we want an eye-in-hand control law") ;
199 vpTRACE(
"\t robot is controlled in the camera frame") ;
203 vpTRACE(
"\t we want to see a point on a point..") ;
204 std::cout << std::endl ;
205 for (i=0 ; i < nbline ; i++)
212 vpTRACE(
"Display task information " ) ;
217 unsigned int iter=0 ;
221 double lambda_av =0.05;
222 double alpha = 0.05 ;
227 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
234 for (i=0 ; i < nbline ; i++)
247 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
251 gain = alpha * exp (-beta * ( task.
getError() ).sumSquare() ) + lambda_av ;
285 vpTRACE(
"Display task information " ) ;
300 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");