ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoAfma6Ellipse2DCamVelocity.cpp
1 /****************************************************************************
2  *
3  * $Id: servoAfma6Ellipse2DCamVelocity.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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  *
42  *****************************************************************************/
43 
44 
65 #include <visp/vpConfig.h>
66 #include <visp/vpDebug.h> // Debug trace
67 #include <stdlib.h>
68 #include <cmath> // std::fabs
69 #include <limits> // numeric_limits
70 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
71 
72 #include <visp/vp1394TwoGrabber.h>
73 #include <visp/vpImage.h>
74 #include <visp/vpDisplay.h>
75 #include <visp/vpDisplayX.h>
76 #include <visp/vpDisplayOpenCV.h>
77 #include <visp/vpDisplayGTK.h>
78 
79 #include <visp/vpMath.h>
80 #include <visp/vpHomogeneousMatrix.h>
81 #include <visp/vpFeatureEllipse.h>
82 #include <visp/vpServo.h>
83 #include <visp/vpFeatureBuilder.h>
84 
85 #include <visp/vpRobotAfma6.h>
86 
87 // Exception
88 #include <visp/vpException.h>
89 #include <visp/vpMatrixException.h>
90 #include <visp/vpServoDisplay.h>
91 
92 #include <visp/vpDot.h>
93 
94 
95 
96 int
97 main()
98 {
99  try
100  {
101  vpServo task ;
102 
107  g.open(I) ;
108  g.acquire(I) ;
109 
110 #ifdef VISP_HAVE_X11
111  vpDisplayX display(I,100,100,"Current image") ;
112 #elif defined(VISP_HAVE_OPENCV)
113  vpDisplayOpenCV display(I,100,100,"Current image") ;
114 #elif defined(VISP_HAVE_GTK)
115  vpDisplayGTK display(I,100,100,"Current image") ;
116 #endif
117 
118  vpDisplay::display(I) ;
119  vpDisplay::flush(I) ;
120 
121  std::cout << std::endl ;
122  std::cout << "-------------------------------------------------------" << std::endl ;
123  std::cout << " Test program for vpServo " <<std::endl ;
124  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
125  std::cout << " Simulation " << std::endl ;
126  std::cout << " task : servo a point " << std::endl ;
127  std::cout << "-------------------------------------------------------" << std::endl ;
128  std::cout << std::endl ;
129 
130 
131  vpDot dot ;
132 
133  dot.setMaxDotSize(0.30) ; // Max dot size is 30 % of the image size
134  // dot.setGraphics(true) ;
135  dot.setComputeMoments(true) ;
136  std::cout << "Click on an ellipse..." << std::endl;
137  dot.initTracking(I) ;
138  vpImagePoint cog = dot.getCog();
140  vpDisplay::flush(I);
141 
142  dot.track(I) ;
143 
144  vpCameraParameters cam ;
145 
146  vpRobotAfma6 robot ;
147 
148  // Update camera parameters
149  robot.getCameraParameters (cam, I);
150 
151  vpTRACE("sets the current position of the visual feature ") ;
152  vpFeatureEllipse c ;
153  vpFeatureBuilder::create(c, cam, dot) ;
154 
155 
156  std::cout << " Learning 0/1 " <<std::endl ;
157  int learning ;
158  std::cin >> learning ;
159  char name[FILENAME_MAX] ;
160  sprintf(name,"dat/ellipse.dat") ;
161  if (learning ==1)
162  {
163  // save the object position
164  vpTRACE("Save the location of the object in a file dat/ellipse.dat") ;
165  std::ofstream f(name) ;
166  f << c.get_s().t() ;
167  f.close() ;
168  exit(1) ;
169  }
170 
171 
172  vpTRACE("sets the desired position of the visual feature ") ;
173  vpFeatureEllipse cd ;
174  std::ifstream f("dat/ellipse.dat") ;
175  double x,y,mu20,mu11,mu02 ;
176  f >> x ; f >> y ; f >> mu20 ; f >> mu11 ; f >> mu02 ;
177  f.close() ;
178  cd.buildFrom(x,y,mu20,mu11,mu02) ;
179  cd.setABC(0,0,10) ;
180 
183 
184  task.addFeature(c,cd) ;
185 
186  task.setLambda(0.01) ;
187 
189  unsigned int iter=0 ;
190  double lambda_av =0.01;
191  double alpha = 0.1 ; //1 ;
192  double beta =3 ; //3 ;
193 
194  std::cout << "alpha 0.7" << std::endl;
195  std::cin >> alpha ;
196  std::cout << "beta 5" << std::endl;
197  std::cin >> beta ;
198  for ( ; ; )
199  {
200  std::cout << "---------------------------------------------" << iter++ <<std::endl ;
201 
202  g.acquire(I) ;
203  vpDisplay::display(I) ;
204 
205  dot.track(I) ;
206 
207  // Get the dot cog
208  cog = dot.getCog();
209 
211 
212  vpFeatureBuilder::create(c,cam, dot);
213  // Compute the adaptative gain (speed up the convergence)
214  double gain ;
215  if (iter>2)
216  {
217  if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
218  gain = lambda_av ;
219  else
220  {
221  gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av;
222  }
223  }
224  else gain = lambda_av ;
225 
226 
227  vpTRACE("%f %f", ( task.getError() ).sumSquare(), gain) ;
228  task.setLambda(gain) ;
229  vpColVector v ;
230  v = task.computeControlLaw() ;
231  std::cout <<"rank " << task.getTaskRank() << std::endl ;
232  vpServoDisplay::display(task,cam,I) ;
233  std::cout << v.t() ;
235 
236  vpDisplay::flush(I) ;
237  vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
238  }
239 
240  vpTRACE("Display task information " ) ;
241  task.print() ;
242  task.kill();
243  }
244  catch (...)
245  {
246  vpERROR_TRACE(" Test failed") ;
247  return 0;
248  }
249 }
250 
251 
252 #else
253 int
254 main()
255 {
256  vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
257 }
258 #endif