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/vpImagePoint.h>
73 #include <visp/vpImageIo.h>
74 #include <visp/vpDisplay.h>
75 #include <visp/vpDisplayX.h>
76 #include <visp/vpDisplayOpenCV.h>
77 #include <visp/vpDisplayGTK.h>
79 #include <visp/vpMath.h>
80 #include <visp/vpHomogeneousMatrix.h>
81 #include <visp/vpFeatureLine.h>
82 #include <visp/vpFeaturePoint.h>
83 #include <visp/vpFeatureDepth.h>
84 #include <visp/vpGenericFeature.h>
85 #include <visp/vpLine.h>
86 #include <visp/vpServo.h>
87 #include <visp/vpFeatureBuilder.h>
88 #include <visp/vpPose.h>
90 #include <visp/vpRobotAfma6.h>
93 #include <visp/vpException.h>
94 #include <visp/vpMatrixException.h>
95 #include <visp/vpServoDisplay.h>
97 #include <visp/vpDot2.h>
98 #include <visp/vpPoint.h>
99 #include <visp/vpHomogeneousMatrix.h>
117 vpDisplayX display(I,100,100,
"Current image") ;
118 #elif defined(VISP_HAVE_OPENCV)
120 #elif defined(VISP_HAVE_GTK)
137 std::cout << std::endl ;
138 std::cout <<
"-------------------------------------------------------" << std::endl ;
139 std::cout <<
" Test program for vpServo " <<std::endl ;
140 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
141 std::cout <<
" Simulation " << std::endl ;
142 std::cout <<
" task : servo a line " << std::endl ;
143 std::cout <<
"-------------------------------------------------------" << std::endl ;
144 std::cout << std::endl ;
150 vpTRACE(
"sets the desired position of the visual feature ") ;
176 vpTRACE(
"Initialization of the tracking") ;
188 for (i=0 ; i < nbline ; i++)
199 for (i=0 ; i < nbline ; i++)
224 for (i=0 ; i < nbline ; i++)
233 vpTRACE(
"sets the current position of the visual feature ") ;
237 double xc = (point[0].
get_x()+point[2].
get_x())/2;
238 double yc = (point[0].
get_y()+point[2].
get_y())/2;
256 vpTRACE(
"\t we want an eye-in-hand control law") ;
257 vpTRACE(
"\t robot is controlled in the camera frame") ;
261 vpTRACE(
"\t we want to see a point on a point..") ;
262 std::cout << std::endl ;
272 vpTRACE(
"Display task information " ) ;
277 unsigned int iter=0 ;
281 double lambda_av =0.05;
282 double alpha = 0.05 ;
287 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
296 for (i=0 ; i < nbline ; i++)
331 for (i = 0; i < nbpoint; i++) pointd[i].display(I, cam,
vpColor::red);
343 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
347 gain = alpha * exp (-beta * ( task.
getError() ).sumSquare() ) + lambda_av ;
381 vpTRACE(
"Display task information " ) ;
396 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");