ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoSimuCylinder.cpp
1 /****************************************************************************
2  *
3  * $Id: servoSimuCylinder.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  * Demonstration of the wireframe simulator with a simple visual servoing
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
49 #include <stdlib.h>
50 
51 #include <visp/vpCameraParameters.h>
52 #include <visp/vpCylinder.h>
53 #include <visp/vpDisplayOpenCV.h>
54 #include <visp/vpDisplayX.h>
55 #include <visp/vpDisplayGTK.h>
56 #include <visp/vpDisplayGDI.h>
57 #include <visp/vpDisplayD3D.h>
58 #include <visp/vpFeatureBuilder.h>
59 #include <visp/vpHomogeneousMatrix.h>
60 #include <visp/vpImage.h>
61 #include <visp/vpImageIo.h>
62 #include <visp/vpIoTools.h>
63 #include <visp/vpMath.h>
64 #include <visp/vpParseArgv.h>
65 #include <visp/vpRobotCamera.h>
66 #include <visp/vpServo.h>
67 #include <visp/vpTime.h>
68 #include <visp/vpVelocityTwistMatrix.h>
69 #include <visp/vpWireFrameSimulator.h>
70 
71 #define GETOPTARGS "dh"
72 
73 #ifdef VISP_HAVE_DISPLAY
74 
83 void usage(const char *name, const char *badparam)
84 {
85  fprintf(stdout, "\n\
86 Demonstration of the wireframe simulator with a simple visual servoing.\n\
87  \n\
88 The visual servoing consists in bringing the camera at a desired position\n\
89 from the object.\n\
90  \n\
91 The visual features used to compute the pose of the camera and \n\
92 thus the control law are two lines. These features are computed thanks \n\
93 to the equation of a cylinder.\n\
94  \n\
95 This demonstration explains also how to move the object around a world \n\
96 reference frame. Here, the movment is a rotation around the x and y axis \n\
97 at a given distance from the world frame. In fact the object trajectory \n\
98 is on a sphere whose center is the origin of the world frame.\n\
99  \n\
100 SYNOPSIS\n\
101  %s [-d] [-h]\n", name);
102 
103  fprintf(stdout, "\n\
104 OPTIONS: \n\
105  -d \n\
106  Turn off the display.\n\
107  \n\
108  -h\n\
109  Print the help.\n");
110 
111  if (badparam)
112  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
113 }
114 
115 
127 bool getOptions(int argc, const char **argv, bool &display)
128 {
129  const char *optarg;
130  int c;
131  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
132 
133  switch (c) {
134  case 'd': display = false; break;
135  case 'h': usage(argv[0], NULL); return false; break;
136 
137  default:
138  usage(argv[0], optarg);
139  return false; break;
140  }
141  }
142 
143  if ((c == 1) || (c == -1)) {
144  // standalone param or error
145  usage(argv[0], NULL);
146  std::cerr << "ERROR: " << std::endl;
147  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
148  return false;
149  }
150 
151  return true;
152 }
153 
154 
155 int
156 main(int argc, const char ** argv)
157 {
158  bool opt_display = true;
159 
160  // Read the command line options
161  if (getOptions(argc, argv, opt_display) == false) {
162  exit (-1);
163  }
164 
165  vpImage<vpRGBa> Iint(480,640,255);
166  vpImage<vpRGBa> Iext(480,640,255);
167 
168 #if defined VISP_HAVE_X11
169  vpDisplayX display[2];
170 #elif defined VISP_HAVE_OPENCV
171  vpDisplayOpenCV display[2];
172 #elif defined VISP_HAVE_GDI
173  vpDisplayGDI display[2];
174 #elif defined VISP_HAVE_D3D9
175  vpDisplayD3D display[2];
176 #elif defined VISP_HAVE_GTK
177  vpDisplayGTK display[2];
178 #endif
179 
180  if (opt_display)
181  {
182  try
183  {
184  // Display size is automatically defined by the image (I) size
185  display[0].init(Iint, 100, 100,"The internal view") ;
186  display[1].init(Iext, 100, 100,"The first external view") ;
187  vpDisplay::setWindowPosition (Iint, 0, 0);
188  vpDisplay::setWindowPosition (Iext, 700, 0);
189  vpDisplay::display(Iint);
190  vpDisplay::flush(Iint);
191  vpDisplay::display(Iext);
192  vpDisplay::flush(Iext);
193  }
194  catch(...)
195  {
196  vpERROR_TRACE("Error while displaying the image") ;
197  exit(-1);
198  }
199  }
200 
201  vpServo task;
202  vpRobotCamera robot ;
203  float sampling_time = 0.040f; // Sampling period in second
204  robot.setSamplingTime(sampling_time);
205 
206  // Set initial position of the object in the camera frame
207  vpHomogeneousMatrix cMo(0,0.1,0.3,vpMath::rad(35),vpMath::rad(25),vpMath::rad(75));
208  // Set desired position of the object in the camera frame
209  vpHomogeneousMatrix cdMo(0.0,0.0,0.5,vpMath::rad(90),vpMath::rad(0),vpMath::rad(0));
210  // Set initial position of the object in the world frame
211  vpHomogeneousMatrix wMo(0.0,0.0,0,0,0,0);
212  // Position of the camera in the world frame
213  vpHomogeneousMatrix wMc, cMw;
214  wMc = wMo * cMo.inverse();
215  cMw = wMc.inverse();
216 
217  // Create a cylinder
218  vpCylinder cylinder(0,0,1,0,0,0,0.1);
219 
220  // Projection of the cylinder
221  cylinder.track(cMo);
222 
223  //Set the current visual feature
224  vpFeatureLine l[2];
227 
228  // Projection of the cylinder
229  cylinder.track(cdMo);
230 
231  vpFeatureLine ld[2];
234 
237 
239  vpVelocityTwistMatrix cVe(cMe);
240  task.set_cVe(cVe);
241 
242  vpMatrix eJe;
243  robot.get_eJe(eJe);
244  task.set_eJe(eJe);
245 
246  for (int i = 0 ; i < 2 ; i++)
247  task.addFeature(l[i],ld[i]) ;
248 
249  task.setLambda(1);
250 
252 
253  // Set the scene
255 
256  // Initialize simulator frames
257  sim.set_fMo( wMo ); // Position of the object in the world reference frame
258  sim.setCameraPositionRelObj(cMo) ; // initial position of the camera
259  sim.setDesiredCameraPosition(cdMo); // desired position of the camera
260 
261  // Set the External camera position
263  sim.setExternalCameraPosition(camMf);
264 
265  // Set the parameters of the cameras (internal and external)
266  vpCameraParameters camera(1000,1000,320,240);
267  sim.setInternalCameraParameters(camera);
268  sim.setExternalCameraParameters(camera);
269 
270  int stop = 10;
271 
272  if (opt_display)
273  {
274  stop = 2500;
275 
276  //Get the internal and external views
277  sim.getInternalImage(Iint);
278  sim.getExternalImage(Iext);
279 
280  //Display the object frame (current and desired position)
281  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
282  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
283 
284  //Display the object frame the world reference frame and the camera frame
285  vpDisplay::displayFrame(Iext,camMf*sim.get_fMo()*cMo.inverse(),camera,0.2,vpColor::none);
286  vpDisplay::displayFrame(Iext,camMf*sim.get_fMo(),camera,0.2,vpColor::none);
287  vpDisplay::displayFrame(Iext,camMf,camera,0.2,vpColor::none);
288 
289  vpDisplay::flush(Iint);
290  vpDisplay::flush(Iext);
291 
292  std::cout << "Click on a display" << std::endl;
293  while (!vpDisplay::getClick(Iint,false) && !vpDisplay::getClick(Iext,false)){};
294  }
295 
296  robot.setPosition( cMw );
297 
298  //Print the task
299  task.print();
300 
301  int iter = 0;
302  vpColVector v ;
303 
304  //Set the secondary task parameters
305  vpColVector e1(6) ; e1 = 0 ;
306  vpColVector e2(6) ; e2 = 0 ;
307  vpColVector proj_e1 ;
308  vpColVector proj_e2 ;
309  iter = 0;
310  double rapport = 0;
311  double vitesse = 0.3;
312  int tempo = 600;
313 
314  while(iter++ < stop)
315  {
316  if (opt_display)
317  {
318  vpDisplay::display(Iint) ;
319  vpDisplay::display(Iext) ;
320  }
321 
322  double t = vpTime::measureTimeMs();
323 
324  robot.get_eJe(eJe) ;
325  task.set_eJe(eJe) ;
326 
327  robot.getPosition(cMw) ;
328  cMo = cMw * wMo;
329 
330  cylinder.track(cMo) ;
333 
334  v = task.computeControlLaw() ;
335 
336  //Compute the velocity with the secondary task
337  if ( iter%tempo < 200 && iter%tempo >= 0)
338  {
339  e2 = 0;
340  e1[0] = -fabs(vitesse) ;
341  proj_e1 = task.secondaryTask(e1);
342  rapport = -vitesse/proj_e1[0];
343  proj_e1 *= rapport ;
344  v += proj_e1 ;
345  }
346 
347  if ( iter%tempo < 300 && iter%tempo >= 200)
348  {
349  e1 = 0;
350  e2[1] = -fabs(vitesse) ;
351  proj_e2 = task.secondaryTask(e2);
352  rapport = -vitesse/proj_e2[1];
353  proj_e2 *= rapport ;
354  v += proj_e2 ;
355  }
356 
357  if ( iter%tempo < 500 && iter%tempo >= 300)
358  {
359  e2 = 0;
360  e1[0] = -fabs(vitesse) ;
361  proj_e1 = task.secondaryTask(e1);
362  rapport = vitesse/proj_e1[0];
363  proj_e1 *= rapport ;
364  v += proj_e1 ;
365  }
366 
367  if ( iter%tempo < 600 && iter%tempo >= 500)
368  {
369  e1 = 0;
370  e2[1] = -fabs(vitesse) ;
371  proj_e2 = task.secondaryTask(e2);
372  rapport = vitesse/proj_e2[1];
373  proj_e2 *= rapport ;
374  v += proj_e2 ;
375  }
376 
378 
379  // Update the simulator frames
380  sim.set_fMo( wMo ); // This line is not really requested since the object doesn't move
381  sim.setCameraPositionRelObj( cMo );
382 
383  if (opt_display)
384  {
385  //Get the internal and external views
386  sim.getInternalImage(Iint);
387  sim.getExternalImage(Iext);
388 
389  //Display the object frame (current and desired position)
390  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
391  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
392 
393  //Display the object frame the world reference frame and the camera frame
397 
398  vpDisplay::flush(Iint);
399  vpDisplay::flush(Iext);
400  }
401 
402  vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
403 
404  std::cout << "|| s - s* || = " << (task.getError() ).sumSquare() <<std::endl ;
405  }
406 
407  task.print() ;
408  task.kill() ;
409 
410  return 0;
411 }
412 #else
413 int
414 main()
415 {
416  vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
417 }
418 
419 #endif