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
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
A cylinder of 80cm length and 10cm radius.
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:379
void setPosition(const vpHomogeneousMatrix &cMw)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:133
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
Definition: vpServo.cpp:444
static const vpColor none
Definition: vpColor.h:179
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:253
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
static double measureTimeMs()
Definition: vpTime.cpp:86
vpColVector secondaryTask(vpColVector &de2dt)
Add a secondary task.
Definition: vpServo.cpp:1053
static int wait(double t0, double t)
Definition: vpTime.cpp:149
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo)
void set_cVe(vpVelocityTwistMatrix &_cVe)
Definition: vpServo.h:230
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1991
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:79
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf)
virtual void setSamplingTime(const double &delta_t)
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:109
void kill()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
vpColVector getError() const
Definition: vpServo.h:301
vpHomogeneousMatrix get_fMo() const
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
virtual void setWindowPosition(int winx, int winy)=0
Class that defines the simplest robot: a free flying camera.
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:203
void set_eJe(vpMatrix &_eJe)
Definition: vpServo.h:238
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void getExternalImage(vpImage< unsigned char > &I)
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay.cpp:368
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
Definition: vpServo.cpp:509
static double rad(double deg)
Definition: vpMath.h:100
void setExternalCameraParameters(const vpCameraParameters &cam)
void getInternalImage(vpImage< unsigned char > &I)
void getPosition(vpHomogeneousMatrix &cMw) const
Class that defines what is a cylinder.
Definition: vpCylinder.h:97
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 setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo)
void get_eJe(vpMatrix &eJe)
vpHomogeneousMatrix inverse() const
void setInternalCameraParameters(const vpCameraParameters &cam)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:258
virtual bool getClick(bool blocking=true)=0
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law descbribed in and .
Definition: vpServo.h:153
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
void set_fMo(const vpHomogeneousMatrix &fMo)
vpHomogeneousMatrix getExternalCameraPosition() const