ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoSimuViper850FourPoints2DCamVelocity.cpp
1 /****************************************************************************
2  *
3  * $Id: servoSimuFourPoints2DPolarCamVelocityDisplay.cpp 2503 2010-02-16 18:55:01Z 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  * Simulation of a 2D visual servoing using 4 points with polar
36  * coordinates as visual feature.
37  *
38  * Authors:
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
43 
60 #include <visp/vpDebug.h>
61 #include <visp/vpConfig.h>
62 
63 #if (defined(WIN32) || defined(VISP_HAVE_PTHREAD)) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI))
64 
65 // We need to use threading capabilities. Thus on Unix-like
66 // platforms, the libpthread third-party library need to be
67 // installed. On Windows, we use the native threading capabilities.
68 
69 #include <stdlib.h>
70 #include <stdio.h>
71 
72 #include <visp/vpCameraParameters.h>
73 #include <visp/vpDisplayX.h>
74 #include <visp/vpDisplayGTK.h>
75 #include <visp/vpDisplayGDI.h>
76 #include <visp/vpFeatureBuilder.h>
77 #include <visp/vpFeaturePoint.h>
78 #include <visp/vpHomogeneousMatrix.h>
79 #include <visp/vpImage.h>
80 #include <visp/vpImagePoint.h>
81 #include <visp/vpIoTools.h>
82 #include <visp/vpMath.h>
83 #include <visp/vpMeterPixelConversion.h>
84 #include <visp/vpParseArgv.h>
85 #include <visp/vpServo.h>
86 #include <visp/vpSimulatorViper850.h>
87 
88 // List of allowed command line options
89 #define GETOPTARGS "cdh"
90 
99 void usage(const char *name, const char *badparam)
100 {
101  fprintf(stdout, "\n\
102 Tests a control law with the following characteristics:\n\
103 - eye-in-hand control\n\
104 - articular velocity are computed\n\
105 - servo on 4 points,\n\
106 - internal and external camera view displays.\n\
107  \n\
108 SYNOPSIS\n\
109  %s [-c] [-d] [-h]\n", name);
110 
111  fprintf(stdout, "\n\
112 OPTIONS: Default\n\
113  -c\n\
114  Disable the mouse click. Useful to automaze the \n\
115  execution of this program without humain intervention.\n\
116  \n\
117  -d \n\
118  Turn off the display.\n\
119  \n\
120  -h\n\
121  Print the help.\n");
122 
123  if (badparam)
124  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
125 }
138 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
139 {
140  const char *optarg;
141  int c;
142  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
143 
144  switch (c) {
145  case 'c': click_allowed = false; break;
146  case 'd': display = false; break;
147  case 'h': usage(argv[0], NULL); return false; break;
148 
149  default:
150  usage(argv[0], optarg);
151  return false; break;
152  }
153  }
154 
155  if ((c == 1) || (c == -1)) {
156  // standalone param or error
157  usage(argv[0], NULL);
158  std::cerr << "ERROR: " << std::endl;
159  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
160  return false;
161  }
162 
163  return true;
164 }
165 
166 int
167 main(int argc, const char ** argv)
168 {
169  bool opt_click_allowed = true;
170  bool opt_display = true;
171 
172  // Read the command line options
173  if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
174  exit (-1);
175  }
176 
177  // We open two displays, one for the internal camera view, the other one for
178  // the external view, using either X11, GTK or GDI.
179 #if defined VISP_HAVE_X11
180  vpDisplayX displayInt;
181 #elif defined VISP_HAVE_GDI
182  vpDisplayGDI displayInt;
183 #elif defined VISP_HAVE_OPENCV
184  vpDisplayOpenCV displayInt;
185 #endif
186 
187  // open a display for the visualization
188 
189  vpImage<unsigned char> Iint(480, 640, 255);
190 
191  if (opt_display) {
192  displayInt.init(Iint,700,0, "Internal view") ;
193  }
194 
195  int i;
196  vpServo task;
197 
198  std::cout << std::endl ;
199  std::cout << "----------------------------------------------" << std::endl ;
200  std::cout << " Test program for vpServo " <<std::endl ;
201  std::cout << " Eye-in-hand task control, articular velocity are computed"
202  << std::endl ;
203  std::cout << " Simulation " << std::endl ;
204  std::cout << " task : servo 4 points " << std::endl ;
205  std::cout << "----------------------------------------------" << std::endl ;
206  std::cout << std::endl ;
207 
208  // sets the initial camera location
209  vpHomogeneousMatrix cMo(-0.05,-0.05,0.7,
210  vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
211 
212 
213  // sets the point coordinates in the object frame
214  vpPoint point[4] ;
215  point[0].setWorldCoordinates(-0.045,-0.045,0) ;
216  point[3].setWorldCoordinates(-0.045,0.045,0) ;
217  point[2].setWorldCoordinates(0.045,0.045,0) ;
218  point[1].setWorldCoordinates(0.045,-0.045,0) ;
219 
220  // computes the point coordinates in the camera frame and its 2D coordinates
221  for (i = 0 ; i < 4 ; i++)
222  point[i].track(cMo) ;
223 
224  // sets the desired position of the point
225  vpFeaturePoint p[4] ;
226  for (i = 0 ; i < 4 ; i++)
227  vpFeatureBuilder::create(p[i],point[i]) ; //retrieve x,y and Z of the vpPoint structure
228 
229  // sets the desired position of the feature point s*
230  vpFeaturePoint pd[4] ;
231 
232  //Desired pose
234 
235  // Projection of the points
236  for (int i = 0 ; i < 4 ; i++)
237  point[i].track(cdMo);
238 
239  for (int i = 0 ; i < 4 ; i++)
240  vpFeatureBuilder::create(pd[i], point[i]);
241 
242  // define the task
243  // - we want an eye-in-hand control law
244  // - articular velocity are computed
247 
248  // - we want to see a point on a point
249  for (i = 0 ; i < 4 ; i++)
250  task.addFeature(p[i],pd[i]) ;
251 
252  // set the gain
253  task.setLambda(0.8) ;
254 
255  // Declaration of the robot
256  vpSimulatorViper850 robot(opt_display);
257 
258  // Initialise the robot and especially the camera
261 
262  // Initialise the object for the display part
264 
265  // Initialise the position of the object relative to the pose of the robot's camera
266  robot.initialiseObjectRelativeToCamera(cMo);
267 
268  // Set the desired position (for the display part)
269  robot.setDesiredCameraPosition(cdMo);
270 
271  // Get the internal robot's camera parameters
272  vpCameraParameters cam;
273  robot.getCameraParameters(cam,Iint);
274 
275  if (opt_display)
276  {
277  //Get the internal view
278  vpDisplay::display(Iint);
279  robot.getInternalView(Iint);
280  vpDisplay::flush(Iint);
281  }
282 
283  // Display task information
284  task.print() ;
285 
286  unsigned int iter=0 ;
287  // loop
288  while(iter++<500)
289  {
290  std::cout << "---------------------------------------------" << iter <<std::endl ;
291  vpColVector v ;
292 
293  //Get the Time at the beginning of the loop
294  double t = vpTime::measureTimeMs();
295 
296  //Get the current pose of the camera
297  cMo = robot.get_cMo();
298 
299  if (iter==1) {
300  std::cout <<"Initial robot position with respect to the object frame:\n";
301  cMo.print();
302  }
303 
304  // new point position
305  for (i = 0 ; i < 4 ; i++)
306  {
307  point[i].track(cMo) ;
308  //retrieve x,y and Z of the vpPoint structure
309  try {
310  vpFeatureBuilder::create(p[i],point[i]) ;
311  }
312  catch(...)
313  {
314  break;
315  }
316  }
317 
318  if (opt_display)
319  {
320  // Get the internal view and display it
321  vpDisplay::display(Iint) ;
322  robot.getInternalView(Iint);
323  vpDisplay::flush(Iint);
324  }
325 
326  if (opt_display && opt_click_allowed && iter == 1)
327  {
328  // suppressed for automate test
329  std::cout << "Click in the internal view window to continue..." << std::endl;
330  vpDisplay::getClick(Iint) ;
331  }
332 
333  // compute the control law
334  v = task.computeControlLaw() ;
335 
336  // send the camera velocity to the controller
338 
339  std::cout << "|| s - s* || " << ( task.getError() ).sumSquare() <<std::endl ;
340 
341  // The main loop has a duration of 10 ms at minimum
342  vpTime::wait(t,10);
343  }
344 
345  // Display task information
346  task.print() ;
347  task.kill();
348 
349  std::cout <<"Final robot position with respect to the object frame:\n";
350  cMo.print();
351 
352  if (opt_display && opt_click_allowed)
353  {
354  // suppressed for automate test
355  std::cout << "Click in the internal view window to end..." << std::endl;
356  vpDisplay::getClick(Iint) ;
357  }
358 }
359 #else
360 int
361 main()
362 {
363  vpERROR_TRACE("You do not have X11, OpenCV or GDI display functionalities or threading capabilities...");
364 }
365 
366 #endif