ViSP  2.6.2
servoSimuCylinder.cpp
1 /****************************************************************************
2  *
3  * $Id: servoSimuCylinder.cpp 3628 2012-03-14 11:04:46Z 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  * Demonstration of the wireframe simulator with a simple visual servoing
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
56 #include <visp/vpImage.h>
57 #include <visp/vpImageIo.h>
58 #include <visp/vpDisplayOpenCV.h>
59 #include <visp/vpDisplayX.h>
60 #include <visp/vpDisplayGTK.h>
61 #include <visp/vpDisplayGDI.h>
62 #include <visp/vpDisplayD3D.h>
63 #include <visp/vpCameraParameters.h>
64 #include <visp/vpTime.h>
65 
66 #include <visp/vpMath.h>
67 #include <visp/vpHomogeneousMatrix.h>
68 #include <visp/vpFeaturePoint.h>
69 #include <visp/vpServo.h>
70 #include <visp/vpRobotCamera.h>
71 #include <visp/vpFeatureBuilder.h>
72 #include <visp/vpParseArgv.h>
73 #include <visp/vpIoTools.h>
74 #include <visp/vpVelocityTwistMatrix.h>
75 #include <visp/vpWireFrameSimulator.h>
76 #include <visp/vpCylinder.h>
77 #include <stdlib.h>
78 #define GETOPTARGS "dh"
79 
80 #if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK))
81 
90 void usage(const char *name, const char *badparam)
91 {
92  fprintf(stdout, "\n\
93 Demonstration of the wireframe simulator with a simple visual servoing.\n\
94  \n\
95 The visual servoing consists in bringing the camera at a desired position\n\
96 from the object.\n\
97  \n\
98 The visual features used to compute the pose of the camera and \n\
99 thus the control law are two lines. These features are computed thanks \n\
100 to the equation of a cylinder.\n\
101  \n\
102 This demonstration explains also how to move the object around a world \n\
103 reference frame. Here, the movment is a rotation around the x and y axis \n\
104 at a given distance from the world frame. In fact the object trajectory \n\
105 is on a sphere whose center is the origin of the world frame.\n\
106  \n\
107 SYNOPSIS\n\
108  %s [-d] [-h]\n", name);
109 
110  fprintf(stdout, "\n\
111 OPTIONS: \n\
112  -d \n\
113  Turn off the display.\n\
114  \n\
115  -h\n\
116  Print the help.\n");
117 
118  if (badparam)
119  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
120 }
121 
122 
134 bool getOptions(int argc, const char **argv, bool &display)
135 {
136  const char *optarg;
137  int c;
138  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
139 
140  switch (c) {
141  case 'd': display = false; break;
142  case 'h': usage(argv[0], NULL); return false; break;
143 
144  default:
145  usage(argv[0], optarg);
146  return false; break;
147  }
148  }
149 
150  if ((c == 1) || (c == -1)) {
151  // standalone param or error
152  usage(argv[0], NULL);
153  std::cerr << "ERROR: " << std::endl;
154  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
155  return false;
156  }
157 
158  return true;
159 }
160 
161 
162 int
163 main(int argc, const char ** argv)
164 {
165  bool opt_display = true;
166 
167  // Read the command line options
168  if (getOptions(argc, argv, opt_display) == false) {
169  exit (-1);
170  }
171 
172  vpImage<vpRGBa> Iint(480,640,255);
173  vpImage<vpRGBa> Iext(480,640,255);
174 
175 #if defined VISP_HAVE_X11
176  vpDisplayX display[2];
177 #elif defined VISP_HAVE_OPENCV
178  vpDisplayOpenCV display[2];
179 #elif defined VISP_HAVE_GDI
180  vpDisplayGDI display[2];
181 #elif defined VISP_HAVE_D3D9
182  vpDisplayD3D display[2];
183 #elif defined VISP_HAVE_GTK
184  vpDisplayGTK display[2];
185 #endif
186 
187  if (opt_display)
188  {
189  try
190  {
191  // Display size is automatically defined by the image (I) size
192  display[0].init(Iint, 100, 100,"The internal view") ;
193  display[1].init(Iext, 100, 100,"The first external view") ;
194  vpDisplay::setWindowPosition (Iint, 0, 0);
195  vpDisplay::setWindowPosition (Iext, 700, 0);
196  vpDisplay::display(Iint);
197  vpDisplay::flush(Iint);
198  vpDisplay::display(Iext);
199  vpDisplay::flush(Iext);
200  }
201  catch(...)
202  {
203  vpERROR_TRACE("Error while displaying the image") ;
204  exit(-1);
205  }
206  }
207 
208  vpServo task;
209  vpRobotCamera robot ;
210  float sampling_time = 0.040f; // Sampling period in second
211  robot.setSamplingTime(sampling_time);
212 
213  //cMo initial
214  vpPoseVector cMoi(0,0.1,0.3,vpMath::rad(35),vpMath::rad(25),vpMath::rad(75));
215 
216  vpHomogeneousMatrix cMo(cMoi);
217 
218  // Create a cylinder
219  vpCylinder cylinder(0,0,1,0,0,0,0.1);
220 
221  // Projection of the cylinder
222  cylinder.track(cMo);
223 
224  //Set the current visual feature
225  vpFeatureLine l[2];
228 
229  //cMo desired
231 
232  // Projection of the cylinder
233  cylinder.track(cdMo);
234 
235  vpFeatureLine ld[2];
238 
241 
243  vpVelocityTwistMatrix cVe(cMe);
244  task.set_cVe(cVe);
245 
246  vpMatrix eJe;
247  robot.get_eJe(eJe);
248  task.set_eJe(eJe);
249 
250  for (int i = 0 ; i < 2 ; i++)
251  task.addFeature(l[i],ld[i]) ;
252 
253  task.setLambda(1);
254 
255 
257 
258  //Set the scene
260 
261  //Set the initial and the desired position of the camera.
262  sim.setCameraPositionRelObj(cMoi) ;
263  sim.setDesiredCameraPosition(cdMo);
264 
265  //Set the External camera position
267  sim.setExternalCameraPosition(camMf);
268 
269  //Move the object in the world reference frame
270  sim.set_fMo(vpHomogeneousMatrix(0.0,0.0,0.0,0,0,0));
271 
272  //Set the parameters of the cameras (internal and external)
273  vpCameraParameters camera(1000,1000,320,240);
274  sim.setInternalCameraParameters(camera);
275  sim.setExternalCameraParameters(camera);
276 
277  int stop = 10;
278 
279  if (opt_display)
280  {
281  stop = 2500;
282 
283  //Get the internal and external views
284  sim.getInternalImage(Iint);
285  sim.getExternalImage(Iext);
286 
287  //Display the object frame (current and desired position)
288  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
289  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
290 
291  //Display the object frame the world reference frame and the camera frame
292  vpDisplay::displayFrame(Iext,camMf*sim.get_fMo()*cMo.inverse(),camera,0.2,vpColor::none);
293  vpDisplay::displayFrame(Iext,camMf*sim.get_fMo(),camera,0.2,vpColor::none);
294  vpDisplay::displayFrame(Iext,camMf,camera,0.2,vpColor::none);
295 
296  vpDisplay::flush(Iint);
297  vpDisplay::flush(Iext);
298 
299  std::cout << "Click on a display" << std::endl;
300  while (!vpDisplay::getClick(Iint,false) && !vpDisplay::getClick(Iext,false)){};
301  }
302 
303  robot.setPosition(sim.get_cMo());
304 
305  //Print the task
306  task.print();
307 
308  int iter = 0;
309  vpColVector v ;
310 
311  //Set the secondary task parameters
312  vpColVector e1(6) ; e1 = 0 ;
313  vpColVector e2(6) ; e2 = 0 ;
314  vpColVector proj_e1 ;
315  vpColVector proj_e2 ;
316  iter = 0;
317  double rapport = 0;
318  double vitesse = 0.3;
319  int tempo = 600;
320 
321  while(iter++ < stop)
322  {
323  if (opt_display)
324  {
325  vpDisplay::display(Iint) ;
326  vpDisplay::display(Iext) ;
327  }
328 
329  double t = vpTime::measureTimeMs();
330 
331  robot.get_eJe(eJe) ;
332  task.set_eJe(eJe) ;
333 
334  robot.getPosition(cMo) ;
335 
336  cylinder.track(cMo) ;
339 
340  v = task.computeControlLaw() ;
341 
342  //Compute the velocity with the secondary task
343  if ( iter%tempo < 200 && iter%tempo >= 0)
344  {
345  e2 = 0;
346  e1[0] = -fabs(vitesse) ;
347  proj_e1 = task.secondaryTask(e1);
348  rapport = -vitesse/proj_e1[0];
349  proj_e1 *= rapport ;
350  v += proj_e1 ;
351  }
352 
353  if ( iter%tempo < 300 && iter%tempo >= 200)
354  {
355  e1 = 0;
356  e2[1] = -fabs(vitesse) ;
357  proj_e2 = task.secondaryTask(e2);
358  rapport = -vitesse/proj_e2[1];
359  proj_e2 *= rapport ;
360  v += proj_e2 ;
361  }
362 
363  if ( iter%tempo < 500 && iter%tempo >= 300)
364  {
365  e2 = 0;
366  e1[0] = -fabs(vitesse) ;
367  proj_e1 = task.secondaryTask(e1);
368  rapport = vitesse/proj_e1[0];
369  proj_e1 *= rapport ;
370  v += proj_e1 ;
371  }
372 
373  if ( iter%tempo < 600 && iter%tempo >= 500)
374  {
375  e1 = 0;
376  e2[1] = -fabs(vitesse) ;
377  proj_e2 = task.secondaryTask(e2);
378  rapport = vitesse/proj_e2[1];
379  proj_e2 *= rapport ;
380  v += proj_e2 ;
381  }
382 
384 
385  //Get the camera position
386  robot.getPosition(cMo) ;
387 
388  //Compute the movement of the object around the world reference frame.
389  vpHomogeneousMatrix a(0.0,0.0,0.0,vpMath::rad(0*iter),0,0);
390  vpHomogeneousMatrix b(0,0.0,0.0,vpMath::rad(0*iter),0,0);
391  vpHomogeneousMatrix c(0,0.0,0.0,0,vpMath::rad(0*iter),0);
392 
393  vpHomogeneousMatrix cMf = cMo*sim.get_fMo().inverse(); //The camera position in the world frame
394 
395  sim.set_fMo(b*c*a); //Move the object in the simulator
396 
397  //Indicates to the task the movement of the object
398  cMo = cMf*b*c*a;
399  robot.setPosition(cMo);
400  sim.setCameraPositionRelObj(cMo);
401 
402  if (opt_display)
403  {
404  //Get the internal and external views
405  sim.getInternalImage(Iint);
406  sim.getExternalImage(Iext);
407 
408  //Display the object frame (current and desired position)
409  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
410  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
411 
412  //Display the object frame the world reference frame and the camera frame
415 
417 
418  vpDisplay::flush(Iint);
419  vpDisplay::flush(Iext);
420  }
421 
422  vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
423 
424  vpTRACE("\t\t || s - s* || ") ;
425  std::cout << (task.getError() ).sumSquare() <<std::endl ;
426  }
427 
428  task.print() ;
429  task.kill() ;
430 
431  return 0;
432 }
433 #else
434 int
435 main()
436 {
437  vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
438 }
439 
440 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void setSamplingTime(const double &delta_t)
Definition: vpRobotCamera.h:87
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
#define vpTRACE
Definition: vpDebug.h:401
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
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:177
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:250
void setCameraPositionRelObj(const vpHomogeneousMatrix cMo)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static double measureTimeMs()
Definition: vpTime.cpp:86
vpColVector secondaryTask(vpColVector &de2dt)
Add a secondary task.
Definition: vpServo.cpp:1055
static int wait(double t0, double t)
Definition: vpTime.cpp:149
void set_cVe(vpVelocityTwistMatrix &_cVe)
Definition: vpServo.h:227
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
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:108
void setExternalCameraPosition(const vpHomogeneousMatrix camMf)
void kill()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
vpColVector getError() const
Definition: vpServo.h:298
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.
Definition: vpRobotCamera.h:65
void getInternalImage(vpImage< vpRGBa > &I)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:186
void set_eJe(vpMatrix &_eJe)
Definition: vpServo.h:235
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 ...
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo)
void getPosition(vpColVector &q)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setPosition(const vpRobot::vpControlFrameType, const vpColVector &)
Set a displacement (frame has to be specified) in position control.
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:346
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
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
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix get_cMo() const
void initScene(vpSceneObject obj, vpSceneDesiredObject desiredObject)
void get_eJe(vpMatrix &_eJe)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:258
void setInternalCameraParameters(const vpCameraParameters cam)
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.
Definition: vpServo.h:150
void getExternalImage(vpImage< vpRGBa > &I)
void setExternalCameraParameters(const vpCameraParameters cam)
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