ViSP  2.6.2
servoAfma4Point2DCamVelocityKalman.cpp
1 /****************************************************************************
2  *
3  * $Id: servoAfma4Point2DCamVelocityKalman.cpp 3619 2012-03-09 17:28:57Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * tests the control law
36  * eye-in-hand control
37  * velocity computed in the camera frame
38  *
39  * Authors:
40  * Eric Marchand
41  * Fabien Spindler
42  *
43  *****************************************************************************/
44 
63 #include <visp/vpConfig.h>
64 #include <visp/vpDebug.h> // Debug trace
65 #include <stdlib.h>
66 #if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394_2))
67 
68 #include <visp/vp1394TwoGrabber.h>
69 #include <visp/vpImage.h>
70 #include <visp/vpDisplay.h>
71 #include <visp/vpDisplayX.h>
72 
73 #include <visp/vpMath.h>
74 #include <visp/vpHomogeneousMatrix.h>
75 #include <visp/vpFeaturePoint.h>
76 #include <visp/vpPoint.h>
77 #include <visp/vpServo.h>
78 #include <visp/vpFeatureBuilder.h>
79 #include <visp/vpRobotAfma4.h>
80 #include <visp/vpIoTools.h>
81 #include <visp/vpException.h>
82 #include <visp/vpMatrixException.h>
83 #include <visp/vpServoDisplay.h>
84 #include <visp/vpParseArgv.h>
85 #include <visp/vpDot2.h>
86 #include <visp/vpAdaptiveGain.h>
87 #include <visp/vpLinearKalmanFilterInstantiation.h>
88 
89 
90 // List of allowed command line options
91 #define GETOPTARGS "hK:l:"
92 
93 typedef enum {
94  K_NONE,
95  K_VELOCITY,
96  K_ACCELERATION
97 } KalmanType;
98 
108 void usage(const char *name, const char *badparam,
109  KalmanType &kalman)
110 {
111  fprintf(stdout, "\n\
112 Tests a control law with the following characteristics:\n\
113 - eye-in-hand control\n\
114 - camera velocity are computed\n\
115 - servo on 1 points.\n\
116 - Kalman filtering\n\
117  \n\
118 SYNOPSIS\n\
119  %s [-K <0|1|2|3>] [-h]\n", name);
120 
121  fprintf(stdout, "\n\
122 OPTIONS: Default\n\
123  -l <%%f> \n\
124  Set the constant gain. By default adaptive gain. \n\
125  \n\
126  -K <0|1|2> %d\n\
127  Kalman filtering:\n\
128  0: none\n\
129  1: velocity model\n\
130  2: acceleration model\n\
131  \n\
132  -h\n\
133  Print the help.\n", (int) kalman);
134 
135  if (badparam) {
136  fprintf(stderr, "ERROR: \n" );
137  fprintf(stderr, "\nBad parameter [%s]\n", badparam);
138  }
139 
140 }
141 
156 bool getOptions(int argc, const char **argv, KalmanType &kalman,
157  bool &doAdaptativeGain,
158  vpAdaptiveGain &lambda) // gain lambda
159 {
160  const char *optarg;
161  int c;
162  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
163 
164  switch (c) {
165  case 'K':
166  kalman = (KalmanType) atoi(optarg);
167  break;
168  case 'l':
169  doAdaptativeGain = false;
170  lambda.initFromConstant( atof(optarg) );
171  break;
172  case 'h': usage(argv[0], NULL, kalman);
173  return false; break;
174 
175  default:
176  usage(argv[0], optarg, kalman);
177  return false; break;
178  }
179  }
180 
181  if ((c == 1) || (c == -1)) {
182  // standalone param or error
183  usage(argv[0], NULL, kalman);
184  std::cerr << "ERROR: " << std::endl;
185  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
186  return false;
187  }
188 
189  return true;
190 }
191 
192 int
193 main(int argc, const char ** argv)
194 {
195  KalmanType opt_kalman = K_NONE;
196  vpAdaptiveGain lambda; // Gain de la commande
197  bool doAdaptativeGain = true; // Compute adaptative gain
198  lambda.initStandard(4, 0.2, 40);
199  int opt_cam_frequency = 60; // 60 Hz
200 
201  // Read the command line options
202  if (getOptions(argc, argv, opt_kalman, doAdaptativeGain, lambda) == false) {
203  return (-1);
204  }
205 
206  // Log file creation in /tmp/$USERNAME/log.dat
207  // This file contains by line:
208  // - the 6 computed cam velocities (m/s, rad/s) to achieve the task
209  // - the 6 mesured joint velocities (m/s, rad/s)
210  // - the 6 mesured joint positions (m, rad)
211  // - the 2 values of s - s*
212  std::string username;
213  // Get the user login name
214  vpIoTools::getUserName(username);
215 
216  // Create a log filename to save velocities...
217  std::string logdirname;
218  logdirname ="/tmp/" + username;
219 
220  // Test if the output path exist. If no try to create it
221  if (vpIoTools::checkDirectory(logdirname) == false) {
222  try {
223  // Create the dirname
224  vpIoTools::makeDirectory(logdirname);
225  }
226  catch (...) {
227  std::cerr << std::endl
228  << "ERROR:" << std::endl;
229  std::cerr << " Cannot create " << logdirname << std::endl;
230  exit(-1);
231  }
232  }
233  std::string logfilename;
234  logfilename = logdirname + "/log.dat";
235 
236  // Open the log file name
237  std::ofstream flog(logfilename.c_str());
238 
239  vpServo task ;
240 
241  try {
243  vp1394TwoGrabber g(false);
245  switch(opt_cam_frequency) {
246  case 15: g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15); break;
247  case 30: g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30); break;
248  case 60: g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60); break;
249  }
250  g.open(I) ;
251 
252  for (int i=0; i < 10; i++) // 10 acquisition to warm up the camera
253  g.acquire(I) ;
254 
255  vpDisplayX display(I,100,100,"Current image") ;
256 
257  vpDisplay::display(I) ;
258  vpDisplay::flush(I) ;
259 
260  std::cout << std::endl ;
261  std::cout << "-------------------------------------------------------" << std::endl ;
262  std::cout << "Test program for target motion compensation using a Kalman filter " <<std::endl ;
263  std::cout << "Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
264  std::cout << "Task : servo a point \n" << std::endl ;
265 
266  // Kalman filtering
267  switch(opt_kalman) {
268  case K_NONE:
269  std::cout << "Servo with no target motion compensation (see -K option)\n";
270  break;
271  case K_VELOCITY:
272  std::cout << "Servo with target motion compensation using a Kalman filter\n"
273  << "with constant velocity modelization (see -K option)\n";
274  break;
275  case K_ACCELERATION:
276  std::cout << "Servo with target motion compensation using a Kalman filter\n"
277  << "with constant acceleration modelization (see -K option)\n";
278  break;
279  }
280  std::cout << "-------------------------------------------------------" << std::endl ;
281  std::cout << std::endl ;
282 
283  vpDot2 dot ;
284 
285  std::cout << "Click on the dot..." << std::endl;
286  dot.setGraphics(true);
287  dot.initTracking(I) ;
288  vpImagePoint cog;
289  cog = dot.getCog();
291  vpDisplay::flush(I);
292 
293  vpRobotAfma4 robot ;
294 
295  double px = 1000;
296  double py = 1000;
297  double u0 = I.getWidth() / 2.;
298  double v0 = I.getHeight() / 2.;
299 
300  vpCameraParameters cam(px, py, u0, v0);
301 
302  // Sets the current position of the visual feature
303  vpFeaturePoint p ;
304  vpFeatureBuilder::create(p, cam, dot) ;
305 
306  // Sets the desired position of the visual feature
307  vpFeaturePoint pd ;
308  pd.buildFrom(0,0,1) ;
309 
310  // Define the task
311  // - we want an eye-in-hand control law
312  // - robot is controlled in the camera frame
313  task.setServo(vpServo::EYEINHAND_CAMERA) ;
314 
315  // - we want to see a point on a point
316  std::cout << std::endl ;
317  task.addFeature(p,pd) ;
318 
319  // - set the gain
320  task.setLambda(lambda) ;
321 
322  // Display task information
323  // task.print() ;
324 
325  //--------------------------------------------------------------------------
327  //--------------------------------------------------------------------------
328 
331 
332  // Initialize the kalman filter
333  unsigned int nsignal = 2; // The two values of dedt
334  double rho = 0.3;
335  vpColVector sigma_state;
336  vpColVector sigma_measure(nsignal);
337  unsigned int state_size = 0; // Kalman state vector size
338 
339  switch(opt_kalman) {
340  case K_VELOCITY: {
341  // Set the constant velocity state model used for the filtering
343  state_size = kalman.getStateSize();
344  sigma_state.resize(state_size*nsignal);
345  sigma_state = 0.00001; // Same state variance for all signals
346  sigma_measure = 0.05; // Same measure variance for all the signals
347  double dummy = 0; // non used parameter dt for the velocity state model
348  kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
349 
350  break;
351  }
352  case K_ACCELERATION: {
353  // Set the constant acceleration state model used for the filtering
355  state_size = kalman.getStateSize();
356  sigma_state.resize(state_size*nsignal);
357  sigma_state = 0.00001; // Same variance for all the signals
358  sigma_measure = 0.05; // Same measure variance for all the signals
359  double dt = 1./opt_cam_frequency;
360  kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt );
361  break;
362  }
363  default:
364  break;
365  }
366 
368 
369  int iter = 0 ;
370 
371  double t_0, t_1, Tv, Tv_0, Tv_1;
372  vpColVector vm(6), vm_0(6);
373  vpColVector v(6), v1(6), v2(6); // robot velocities
374  //task error
375  vpColVector err(2), err_0(2), err_1(2);
376  vpColVector dedt_filt(2), dedt_mes(2);
377 
378 
379  t_1 = vpTime::measureTimeMs(); // t_1: time at previous iter
380 
381  Tv_0 = 0;
382 
383  //
384  // Warning: In all varaible names,
385  // _0 means the value for the current iteration (t=0)
386  // _1 means the value for the previous iteration (t=-1)
387  // _2 means the value for the previous previous iteration (t=-2)
388  //
389  std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
390  for ( ; ; ) {
391  t_0 = vpTime::measureTimeMs(); // t_0: current time
392  // Temps de la boucle d'asservissement
393  Tv = (double)(t_0 - t_1) / 1000.0; //temps d'une iteration en s !
394  // std::cout << "time iter : " << Tv << std::endl;
395 
396  // Update time for next iteration
397  t_1 = t_0;
398 
400 
401  // Acquire a new image from the camera
402  g.acquire(I) ;
403 
404  // Display this image
405  vpDisplay::display(I) ;
406 
407  // Achieve the tracking of the dot in the image
408  dot.track(I) ;
409  vpImagePoint cog = dot.getCog();
410 
411  // Display a green cross at the center of gravity position in the image
413 
414  // Update the point feature from the dot location
415  vpFeatureBuilder::create(p, cam, dot);
416 
417  //----------------------------------------------------------------------
419  //----------------------------------------------------------------------
420  vm_0 = vm;
421 
422  // Update current loop time and previous one
423  Tv_1 = Tv_0;
424  Tv_0 = Tv;
425 
426  // Compute the visual servoing skew vector
427  v1 = task.computeControlLaw() ;
428 
429  err = task.error;
430 
432  if (iter==0){
433  err_0 = 0;
434  err_1 = 0;
435  dedt_mes = 0;
436  dedt_filt = 0;
437  }
438  else{
439  err_1 = err_0;
440  err_0 = err;
441 
442  dedt_mes = (err_0 - err_1)/(Tv_1) - task.J1*vm_0;
443  }
445  if (iter <= 1){
446  dedt_mes = 0;
447  }
448 
449  //----------------------------------------------------------------------
450  //----------------------- Kalman Filter Equations ----------------------
451  //----------------------------------------------------------------------
452  // Kalman filtering
453  switch(opt_kalman) {
454  case K_NONE:
455  dedt_filt = 0;
456  break;
457  case K_VELOCITY:
458  case K_ACCELERATION:
459  kalman.filter(dedt_mes);
460  for (unsigned int i=0; i < nsignal; i++) {
461  dedt_filt[i] = kalman.Xest[i*state_size];
462  }
463  break;
464  }
465 
467  vpMatrix J1p = task.getTaskJacobianPseudoInverse();
468  v2 = - J1p*dedt_filt;
469  // std::cout << "task J1p: " << J1p.t() << std::endl ;
470  // std::cout << "dedt_filt: " << dedt_filt.t() << std::endl ;
471 
472  v = v1 + v2;
473 
474  // Display the current and desired feature points in the image display
475  vpServoDisplay::display(task, cam, I) ;
476 
477 
478  // std::cout << "v2 : " << v2.t() << std::endl ;
479  // std::cout << "v1 : " << v1.t() << std::endl ;
480 
481  // std::cout << "v : " << v.t();
482 
483  // Apply the camera velocities to the robot
485 
486  // Save loop time
487  flog << Tv_0 << " ";
488 
489  // Save velocities applied to the robot in the log file
490  // v[0], v[1], v[2] correspond to camera translation velocities in m/s
491  // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
492  flog << v[0] << " " << v[1] << " " << v[2] << " "
493  << v[3] << " " << v[4] << " " << v[5] << " ";
494 
495  // Save feature error (s-s*) for the feature point. For this feature
496  // point, we have 2 errors (along x and y axis). This error is expressed
497  // in meters in the camera frame
498  flog << task.error[0] << " " << task.error[1] << " ";
499 
500  // Save feature error (s-s*) in pixels in the image.
501  flog << cog.get_u()-cam.get_u0() << " "
502  << cog.get_v()-cam.get_v0() << " ";
503 
504  // Save de/dt
505  flog << dedt_mes[0] << " " << dedt_mes[1] << " ";
506 
507  // Save de/dt filtered
508  flog << dedt_filt[0] << " " << dedt_filt[1] << " ";
509 
510  flog << std::endl;
511 
512  // Flush the display
513  vpDisplay::flush(I);
514 
515  iter ++;
516 
517  }
518 
519  flog.close() ; // Close the log file
520 
521  // Display task information
522  task.print() ;
523 
524  // Kill the task
525  task.kill();
526 
527  return 0;
528  }
529  catch (...) {
530  flog.close() ; // Close the log file
531 
532  // Kill the task
533  task.kill();
534 
535  vpERROR_TRACE(" Test failed") ;
536  return 0;
537  }
538 }
539 
540 
541 #else
542 int
543 main()
544 {
545  vpERROR_TRACE("You do not have an afma4 robot or a firewire framegrabber connected to your computer...");
546 }
547 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
double get_v() const
Definition: vpImagePoint.h:250
Adaptive gain computation.
unsigned int getStateSize()
double get_u0() const
static void display(vpServo &s, const vpCameraParameters &cam, vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:289
unsigned int getWidth() const
Definition: vpImage.h:154
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
#define vpERROR_TRACE
Definition: vpDebug.h:379
void initFromConstant(double lambda)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
double get_u() const
Definition: vpImagePoint.h:239
Define the X11 console to display images.
Definition: vpDisplayX.h:152
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static double measureTimeMs()
Definition: vpTime.cpp:86
vpColVector Xest
static const vpColor green
Definition: vpColor.h:168
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:114
void track(const vpImage< unsigned char > &I)
Definition: vpDot2.cpp:439
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1964
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:79
vpImagePoint getCog() const
Definition: vpDot2.h:254
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:358
Initialize the velocity controller.
Definition: vpRobot.h:70
double get_v0() const
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:186
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Generic class defining intrinsic camera parameters.
static std::string getUserName()
Definition: vpIoTools.cpp:136
void buildFrom(const double x, const double y, const double Z)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Control of Irisa's cylindrical robot named Afma4.
Definition: vpRobotAfma4.h:181
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:240
unsigned int getHeight() const
Definition: vpImage.h:145
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law.
Definition: vpServo.h:150
void initStandard(double en_zero, double en_infini, double pente_en_zero)
void setGraphics(const bool activate)
Definition: vpDot2.h:178
static const vpColor blue
Definition: vpColor.h:171
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94