57 #include <visp/vpConfig.h>
58 #include <visp/vpDebug.h>
66 #if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
68 #include <visp/vp1394TwoGrabber.h>
69 #include <visp/vpImage.h>
70 #include <visp/vpMath.h>
71 #include <visp/vpHomogeneousMatrix.h>
72 #include <visp/vpFeaturePoint.h>
73 #include <visp/vpPoint.h>
74 #include <visp/vpServo.h>
75 #include <visp/vpFeatureBuilder.h>
76 #include <visp/vpRobotViper850.h>
77 #include <visp/vpIoTools.h>
78 #include <visp/vpException.h>
79 #include <visp/vpMatrixException.h>
80 #include <visp/vpServoDisplay.h>
81 #include <visp/vpImageIo.h>
82 #include <visp/vpDot2.h>
83 #include <visp/vpAdaptiveGain.h>
84 #include <visp/vpLinearKalmanFilterInstantiation.h>
85 #include <visp/vpDisplay.h>
86 #include <visp/vpDisplayX.h>
87 #include <visp/vpDisplayOpenCV.h>
88 #include <visp/vpDisplayGTK.h>
104 std::string logdirname;
105 logdirname =
"/tmp/" + username;
114 std::cerr << std::endl
115 <<
"ERROR:" << std::endl;
116 std::cerr <<
" Cannot create " << logdirname << std::endl;
120 std::string logfilename;
121 logfilename = logdirname +
"/log.dat";
124 std::ofstream flog(logfilename.c_str());
133 unsigned int nsignal = 2;
137 unsigned int state_size = 0;
141 sigma_state.
resize(state_size*nsignal);
142 sigma_state = 0.00001;
143 sigma_measure = 0.05;
145 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
165 double Tloop = 1./80.f;
179 #elif defined(VISP_HAVE_OPENCV)
181 #elif defined(VISP_HAVE_GTK)
193 for (
int i=0; i< 10; i++)
196 std::cout <<
"Click on a dot..." << std::endl;
223 task.addFeature(p,pd) ;
228 task.setLambda(lambda) ;
236 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
243 dc1394video_frame_t *frame = NULL;
252 Tv = (double)(t_0 - t_1) / 1000.0;
260 frame = g.dequeue(I);
278 v1 = task.computeControlLaw() ;
281 err = task.getError();
289 vpMatrix J1 = task.getTaskJacobian();
290 dedt_mes = (err - err_1)/(Tv) - J1 *vm;
299 for (
unsigned int i=0; i < nsignal; i++) {
300 dedt_filt[i] = kalman.
Xest[i*state_size];
305 vpMatrix J1p = task.getTaskJacobianPseudoInverse();
306 v2 = - J1p*dedt_filt;
324 std::cout <<
"Tracking failed... Stop the robot." << std::endl;
336 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" "
337 << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
347 flog << qvel[0] <<
" " << qvel[1] <<
" " << qvel[2] <<
" "
348 << qvel[3] <<
" " << qvel[4] <<
" " << qvel[5] <<
" ";
358 flog << q[0] <<
" " << q[1] <<
" " << q[2] <<
" "
359 << q[3] <<
" " << q[4] <<
" " << q[5] <<
" ";
364 flog << ( task.getError() ).t() << std::endl;
394 vpERROR_TRACE(
"You do not have a Viper robot or a firewire framegrabber connected to your computer...");