56 #include <visp/vpConfig.h>
57 #include <visp/vpDebug.h>
67 #if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
69 #include <visp/vp1394TwoGrabber.h>
70 #include <visp/vpImage.h>
71 #include <visp/vpDisplay.h>
72 #include <visp/vpDisplayX.h>
73 #include <visp/vpDisplayOpenCV.h>
74 #include <visp/vpDisplayGTK.h>
75 #include <visp/vpMath.h>
76 #include <visp/vpHomogeneousMatrix.h>
77 #include <visp/vpFeaturePoint.h>
78 #include <visp/vpPoint.h>
79 #include <visp/vpServo.h>
80 #include <visp/vpFeatureBuilder.h>
81 #include <visp/vpRobotViper850.h>
82 #include <visp/vpIoTools.h>
83 #include <visp/vpException.h>
84 #include <visp/vpMatrixException.h>
85 #include <visp/vpServoDisplay.h>
86 #include <visp/vpDot2.h>
87 #include <visp/vpPlot.h>
108 double Tloop = 1./60.f;
119 std::cout <<
"Tloop: " << Tloop << std::endl;
122 vpDisplayX display(I,800,100,
"Current image") ;
123 #elif defined(VISP_HAVE_OPENCV)
125 #elif defined(VISP_HAVE_GTK)
142 for (
unsigned int i=0 ; i < 6 ; i++)
144 Qmin[i] = jointMin[i] + 0.5*rho*(jointMax[i]-jointMin[i]) ;
145 Qmax[i] = jointMax[i] - 0.5*rho*(jointMax[i]-jointMin[i]) ;
147 Qmiddle = (Qmin + Qmax) /2.;
150 for (
unsigned int i=0 ; i < 6 ; i++) {
151 tQmin[i]=Qmin[i]+ 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
152 tQmax[i]=Qmax[i]- 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
164 plot.initGraph(0, 10);
165 plot.initGraph(1, 6);
172 plot.initRange(0,0,200,1,-1.2,1.2,0.1);
173 plot.setTitle(0,
"Joint behavior");
174 plot.initRange(1,0,200,1,-0.01,0.01,0.05);
175 plot.setTitle(1,
"Joint velocity");
179 for (
unsigned int i=0; i < 6; i++) {
180 sprintf(legend,
"q%d", i+1);
181 plot.setLegend(0, i, legend);
182 sprintf(legend,
"q%d", i+1);
183 plot.setLegend(1, i, legend);
185 plot.setLegend(0, 6,
"tQmin");
186 plot.setLegend(0, 7,
"tQmax");
187 plot.setLegend(0, 8,
"Qmin");
188 plot.setLegend(0, 9,
"Qmax");
195 plot.setColor(0, 4,
vpColor(0, 128, 0));
197 for (
unsigned int i= 6; i < 10; i++)
205 plot.setColor(1, 4,
vpColor(0, 128, 0));
210 std::cout <<
"Click on a dot..." << std::endl;
237 std::cout << cVe <<std::endl ;
246 std::cout << std::endl ;
261 dc1394video_frame_t *frame = NULL;
263 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
270 Tv = (double)(t_0 - t_1) / 1000.0;
271 std::cout <<
"Tv: " << Tv << std::endl;
277 frame = g.dequeue(I);
310 qpre += -lambda*prim_task*(4*Tloop) ;
314 unsigned int npb =0 ;
315 for (
unsigned int i=0 ; i < 6 ;i++) {
317 if (fabs(Qmin[i]-q[i]) > fabs(Qmin[i]-qpre[i])) {
319 std::cout <<
"Joint " << i <<
" near limit " << std::endl ;
322 if (fabs(Qmax[i]-q[i]) > fabs(Qmax[i]-qpre[i])) {
324 std::cout <<
"Joint " << i <<
" near limit " << std::endl ;
334 unsigned int dimKernelL = kernelJ1.
getCols() ;
342 for (
unsigned int j=0 ; j < 6 ; j++)
344 if (std::fabs(pb[j]-1) <= std::numeric_limits<double>::epsilon()) {
345 for (
unsigned int i=0 ; i < dimKernelL ; i++)
346 E[k][i] = kernelJ1[j][i] ;
348 S[k] = -prim_task[j] ;
353 Ep = E.
t()*(E*E.t()).pseudoInverse() ;
365 v = -lambda * (prim_task + e2);
377 for (
unsigned int i=0 ; i < 6 ; i++) {
378 data[i] = (q[i] - Qmiddle[i]) ;
379 data[i] /= (Qmax[i] - Qmin[i]) ;
382 unsigned int joint = 2;
383 data[6] = 2*(tQmin[joint]-Qmiddle[joint])/(Qmax[joint] - Qmin[joint]) ;
384 data[7] = 2*(tQmax[joint]-Qmiddle[joint])/(Qmax[joint] - Qmin[joint]) ;
385 data[8] = -1 ; data[9] = 1 ;
387 plot.plot(0, iter, data);
388 plot.plot(1, iter, v);
416 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");