62 #include <visp/vpConfig.h>
63 #include <visp/vpDebug.h>
71 #if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
73 #include <visp/vp1394TwoGrabber.h>
74 #include <visp/vpImage.h>
75 #include <visp/vpDisplay.h>
76 #include <visp/vpDisplayX.h>
77 #include <visp/vpDisplayOpenCV.h>
78 #include <visp/vpDisplayGTK.h>
79 #include <visp/vpMath.h>
80 #include <visp/vpHomogeneousMatrix.h>
81 #include <visp/vpFeaturePoint.h>
82 #include <visp/vpPoint.h>
83 #include <visp/vpServo.h>
84 #include <visp/vpFeatureBuilder.h>
85 #include <visp/vpRobotViper850.h>
86 #include <visp/vpIoTools.h>
87 #include <visp/vpException.h>
88 #include <visp/vpMatrixException.h>
89 #include <visp/vpServoDisplay.h>
90 #include <visp/vpDot2.h>
91 #include <visp/vpPlot.h>
113 vpDisplayX display(I,800,100,
"Current image") ;
114 #elif defined(VISP_HAVE_OPENCV)
116 #elif defined(VISP_HAVE_GTK)
133 for (
unsigned int i=0 ; i < 6 ; i++)
135 Qmin[i] = jointMin[i] + 0.5*rho*(jointMax[i]-jointMin[i]) ;
136 Qmax[i] = jointMax[i] - 0.5*rho*(jointMax[i]-jointMin[i]) ;
138 Qmiddle = (Qmin + Qmax) /2.;
141 for (
unsigned int i=0 ; i < 6 ; i++) {
142 tQmin[i]=Qmin[i]+ 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
143 tQmax[i]=Qmax[i]- 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
155 plot.initGraph(0, 10);
157 plot.initGraph(1, 1);
163 plot.initRange(0, 0., 200., -1.2, 1.2);
164 plot.setTitle(0,
"Joint behavior");
171 plot.initRange(1, 0., 200., 0., 1e-4);
172 plot.setTitle(1,
"Cost function");
176 for (
unsigned int i=0; i < 6; i++) {
177 sprintf(legend,
"q%d", i+1);
178 plot.setLegend(0, i, legend);
180 plot.setLegend(0, 6,
"tQmin");
181 plot.setLegend(0, 7,
"tQmax");
182 plot.setLegend(0, 8,
"Qmin");
183 plot.setLegend(0, 9,
"Qmax");
190 plot.setColor(0, 4,
vpColor(0, 128, 0));
192 for (
unsigned int i= 6; i < 10; i++)
196 plot.setLegend(1, 0,
"h_s");
201 std::cout <<
" Give the parameters beta (1) : ";
207 std::cout <<
"Click on a dot..." << std::endl;
234 std::cout << cVe <<std::endl ;
243 std::cout << std::endl ;
255 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
297 for (
unsigned int i=0 ; i < 6 ; i++)
300 if (q[i] > tQmax[i]) S = q[i] - tQmax[i] ;
301 if (q[i] < tQmin[i]) S = q[i] - tQmin[i] ;
302 double D = (Qmax[i]-Qmin[i]) ;
309 std::cout <<
"Cost function h_s: " << h_s << std::endl;
315 v = prim_task + sec_task;
327 for (
unsigned int i=0 ; i < 6 ; i++) {
328 data[i] = (q[i] - Qmiddle[i]) ;
329 data[i] /= (Qmax[i] - Qmin[i]) ;
332 unsigned int joint = 2;
333 data[6] = 2*(tQmin[joint]-Qmiddle[joint])/(Qmax[joint] - Qmin[joint]) ;
334 data[7] = 2*(tQmax[joint]-Qmiddle[joint])/(Qmax[joint] - Qmin[joint]) ;
335 data[8] = -1 ; data[9] = 1 ;
336 plot.plot(0, iter, data);
337 plot.plot(1, 0, iter, h_s);
360 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");