67 #include <visp/vpConfig.h>
68 #include <visp/vpDebug.h>
72 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
74 #include <visp/vp1394TwoGrabber.h>
75 #include <visp/vpImage.h>
76 #include <visp/vpImageIo.h>
77 #include <visp/vpDisplay.h>
78 #include <visp/vpDisplayX.h>
79 #include <visp/vpDisplayOpenCV.h>
80 #include <visp/vpDisplayGTK.h>
82 #include <visp/vpMath.h>
83 #include <visp/vpHomogeneousMatrix.h>
84 #include <visp/vpFeatureLine.h>
85 #include <visp/vpLine.h>
86 #include <visp/vpMeLine.h>
87 #include <visp/vpServo.h>
88 #include <visp/vpFeatureBuilder.h>
90 #include <visp/vpRobotAfma6.h>
93 #include <visp/vpException.h>
94 #include <visp/vpMatrixException.h>
95 #include <visp/vpServoDisplay.h>
112 vpDisplayX display(I,100,100,
"Current image") ;
113 #elif defined(VISP_HAVE_OPENCV)
115 #elif defined(VISP_HAVE_GTK)
124 std::cout << std::endl ;
125 std::cout <<
"-------------------------------------------------------" << std::endl ;
126 std::cout <<
" Test program for vpServo " <<std::endl ;
127 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
128 std::cout <<
" Simulation " << std::endl ;
129 std::cout <<
" task : servo a point " << std::endl ;
130 std::cout <<
"-------------------------------------------------------" << std::endl ;
131 std::cout << std::endl ;
145 vpTRACE(
"The two lines to track must be parallels ") ;
147 for (i=0 ; i < nbline ; i++)
163 vpTRACE(
"sets the current position of the visual feature ") ;
165 for (i=0 ; i < nbline ; i++)
168 vpTRACE(
"sets the desired position of the visual feature ") ;
180 lined[0].
setRho(-fabs(lined[0].getRho()));
182 lined[1].
setRho(-fabs(lined[1].getRho()));
190 vpTRACE(
"\t we want an eye-in-hand control law") ;
191 vpTRACE(
"\t robot is controlled in the camera frame") ;
194 vpTRACE(
"\t we want to see a point on a point..") ;
195 std::cout << std::endl ;
196 for (i=0 ; i < nbline ; i++)
203 vpTRACE(
"Display task information " ) ;
209 unsigned int iter=0 ;
214 double lambda_av =0.05;
220 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
227 for (i=0 ; i < nbline ; i++)
243 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
247 gain = alpha * exp (-beta * ( task.
getError() ).sumSquare() ) + lambda_av ;
269 vpTRACE(
"Display task information " ) ;
284 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");