ViSP  3.0.0
servoSimuCylinder.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Demonstration of the wireframe simulator with a simple visual servoing
32  *
33  * Authors:
34  * Nicolas Melchior
35  *
36  *****************************************************************************/
37 
45 #include <stdlib.h>
46 
47 #include <visp3/core/vpCameraParameters.h>
48 #include <visp3/core/vpCylinder.h>
49 #include <visp3/gui/vpDisplayOpenCV.h>
50 #include <visp3/gui/vpDisplayX.h>
51 #include <visp3/gui/vpDisplayGTK.h>
52 #include <visp3/gui/vpDisplayGDI.h>
53 #include <visp3/gui/vpDisplayD3D.h>
54 #include <visp3/visual_features/vpFeatureBuilder.h>
55 #include <visp3/core/vpHomogeneousMatrix.h>
56 #include <visp3/core/vpImage.h>
57 #include <visp3/io/vpImageIo.h>
58 #include <visp3/core/vpIoTools.h>
59 #include <visp3/core/vpMath.h>
60 #include <visp3/io/vpParseArgv.h>
61 #include <visp3/robot/vpSimulatorCamera.h>
62 #include <visp3/vs/vpServo.h>
63 #include <visp3/core/vpTime.h>
64 #include <visp3/core/vpVelocityTwistMatrix.h>
65 #include <visp3/robot/vpWireFrameSimulator.h>
66 
67 #define GETOPTARGS "dh"
68 
69 #ifdef VISP_HAVE_DISPLAY
70 void usage(const char *name, const char *badparam);
71 bool getOptions(int argc, const char **argv, bool &display);
72 
81 void usage(const char *name, const char *badparam)
82 {
83  fprintf(stdout, "\n\
84 Demonstration of the wireframe simulator with a simple visual servoing.\n\
85  \n\
86 The visual servoing consists in bringing the camera at a desired position\n\
87 from the object.\n\
88  \n\
89 The visual features used to compute the pose of the camera and \n\
90 thus the control law are two lines. These features are computed thanks \n\
91 to the equation of a cylinder.\n\
92  \n\
93 This demonstration explains also how to move the object around a world \n\
94 reference frame. Here, the movment is a rotation around the x and y axis \n\
95 at a given distance from the world frame. In fact the object trajectory \n\
96 is on a sphere whose center is the origin of the world frame.\n\
97  \n\
98 SYNOPSIS\n\
99  %s [-d] [-h]\n", name);
100 
101  fprintf(stdout, "\n\
102 OPTIONS: \n\
103  -d \n\
104  Turn off the display.\n\
105  \n\
106  -h\n\
107  Print the help.\n");
108 
109  if (badparam)
110  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
111 }
112 
113 
125 bool getOptions(int argc, const char **argv, bool &display)
126 {
127  const char *optarg_;
128  int c;
129  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
130 
131  switch (c) {
132  case 'd': display = false; break;
133  case 'h': usage(argv[0], NULL); return false; break;
134 
135  default:
136  usage(argv[0], optarg_);
137  return false; break;
138  }
139  }
140 
141  if ((c == 1) || (c == -1)) {
142  // standalone param or error
143  usage(argv[0], NULL);
144  std::cerr << "ERROR: " << std::endl;
145  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
146  return false;
147  }
148 
149  return true;
150 }
151 
152 
153 int
154 main(int argc, const char ** argv)
155 {
156  try {
157  bool opt_display = true;
158 
159  // Read the command line options
160  if (getOptions(argc, argv, opt_display) == false) {
161  exit (-1);
162  }
163 
164  vpImage<vpRGBa> Iint(480,640,255);
165  vpImage<vpRGBa> Iext(480,640,255);
166 
167 #if defined VISP_HAVE_X11
168  vpDisplayX display[2];
169 #elif defined VISP_HAVE_OPENCV
170  vpDisplayOpenCV display[2];
171 #elif defined VISP_HAVE_GDI
172  vpDisplayGDI display[2];
173 #elif defined VISP_HAVE_D3D9
174  vpDisplayD3D display[2];
175 #elif defined VISP_HAVE_GTK
176  vpDisplayGTK display[2];
177 #endif
178 
179  if (opt_display)
180  {
181  // Display size is automatically defined by the image (I) size
182  display[0].init(Iint, 100, 100,"The internal view") ;
183  display[1].init(Iext, 100, 100,"The first external view") ;
184  vpDisplay::setWindowPosition (Iint, 0, 0);
185  vpDisplay::setWindowPosition (Iext, 700, 0);
186  vpDisplay::display(Iint);
187  vpDisplay::flush(Iint);
188  vpDisplay::display(Iext);
189  vpDisplay::flush(Iext);
190  }
191 
192  vpServo task;
193  vpSimulatorCamera robot ;
194  float sampling_time = 0.040f; // Sampling period in second
195  robot.setSamplingTime(sampling_time);
196 
197  // Set initial position of the object in the camera frame
198  vpHomogeneousMatrix cMo(0,0.1,0.3,vpMath::rad(35),vpMath::rad(25),vpMath::rad(75));
199  // Set desired position of the object in the camera frame
200  vpHomogeneousMatrix cdMo(0.0,0.0,0.5,vpMath::rad(90),vpMath::rad(0),vpMath::rad(0));
201  // Set initial position of the object in the world frame
202  vpHomogeneousMatrix wMo(0.0,0.0,0,0,0,0);
203  // Position of the camera in the world frame
205  wMc = wMo * cMo.inverse();
206 
207  // Create a cylinder
208  vpCylinder cylinder(0,0,1,0,0,0,0.1);
209 
210  // Projection of the cylinder
211  cylinder.track(cMo);
212 
213  //Set the current visual feature
214  vpFeatureLine l[2];
217 
218  // Projection of the cylinder
219  cylinder.track(cdMo);
220 
221  vpFeatureLine ld[2];
224 
227 
229  vpVelocityTwistMatrix cVe(cMe);
230  task.set_cVe(cVe);
231 
232  vpMatrix eJe;
233  robot.get_eJe(eJe);
234  task.set_eJe(eJe);
235 
236  for (int i = 0 ; i < 2 ; i++)
237  task.addFeature(l[i],ld[i]) ;
238 
239  task.setLambda(1);
240 
242 
243  // Set the scene
245 
246  // Initialize simulator frames
247  sim.set_fMo( wMo ); // Position of the object in the world reference frame
248  sim.setCameraPositionRelObj(cMo) ; // initial position of the camera
249  sim.setDesiredCameraPosition(cdMo); // desired position of the camera
250 
251  // Set the External camera position
253  sim.setExternalCameraPosition(camMf);
254 
255  // Set the parameters of the cameras (internal and external)
256  vpCameraParameters camera(1000,1000,320,240);
257  sim.setInternalCameraParameters(camera);
258  sim.setExternalCameraParameters(camera);
259 
260  int stop = 10;
261 
262  if (opt_display)
263  {
264  stop = 2500;
265 
266  //Get the internal and external views
267  sim.getInternalImage(Iint);
268  sim.getExternalImage(Iext);
269 
270  //Display the object frame (current and desired position)
271  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
272  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
273 
274  //Display the object frame the world reference frame and the camera frame
275  vpDisplay::displayFrame(Iext,camMf*sim.get_fMo()*cMo.inverse(),camera,0.2,vpColor::none);
276  vpDisplay::displayFrame(Iext,camMf*sim.get_fMo(),camera,0.2,vpColor::none);
277  vpDisplay::displayFrame(Iext,camMf,camera,0.2,vpColor::none);
278 
279  vpDisplay::flush(Iint);
280  vpDisplay::flush(Iext);
281 
282  std::cout << "Click on a display" << std::endl;
283  while (!vpDisplay::getClick(Iint,false) && !vpDisplay::getClick(Iext,false)){};
284  }
285 
286  robot.setPosition( wMc );
287 
288  //Print the task
289  task.print();
290 
291  int iter = 0;
292  vpColVector v ;
293 
294  //Set the secondary task parameters
295  vpColVector e1(6) ; e1 = 0 ;
296  vpColVector e2(6) ; e2 = 0 ;
297  vpColVector proj_e1 ;
298  vpColVector proj_e2 ;
299  iter = 0;
300  double rapport = 0;
301  double vitesse = 0.3;
302  int tempo = 600;
303 
304  while(iter++ < stop)
305  {
306  if (opt_display)
307  {
308  vpDisplay::display(Iint) ;
309  vpDisplay::display(Iext) ;
310  }
311 
312  double t = vpTime::measureTimeMs();
313 
314  robot.get_eJe(eJe) ;
315  task.set_eJe(eJe) ;
316 
317  wMc = robot.getPosition();
318  cMo = wMc.inverse() * wMo;
319 
320  cylinder.track(cMo) ;
323 
324  v = task.computeControlLaw() ;
325 
326  //Compute the velocity with the secondary task
327  if ( iter%tempo < 200 && iter%tempo >= 0)
328  {
329  e2 = 0;
330  e1[0] = -fabs(vitesse) ;
331  proj_e1 = task.secondaryTask(e1);
332  rapport = -vitesse/proj_e1[0];
333  proj_e1 *= rapport ;
334  v += proj_e1 ;
335  }
336 
337  if ( iter%tempo < 300 && iter%tempo >= 200)
338  {
339  e1 = 0;
340  e2[1] = -fabs(vitesse) ;
341  proj_e2 = task.secondaryTask(e2);
342  rapport = -vitesse/proj_e2[1];
343  proj_e2 *= rapport ;
344  v += proj_e2 ;
345  }
346 
347  if ( iter%tempo < 500 && iter%tempo >= 300)
348  {
349  e2 = 0;
350  e1[0] = -fabs(vitesse) ;
351  proj_e1 = task.secondaryTask(e1);
352  rapport = vitesse/proj_e1[0];
353  proj_e1 *= rapport ;
354  v += proj_e1 ;
355  }
356 
357  if ( iter%tempo < 600 && iter%tempo >= 500)
358  {
359  e1 = 0;
360  e2[1] = -fabs(vitesse) ;
361  proj_e2 = task.secondaryTask(e2);
362  rapport = vitesse/proj_e2[1];
363  proj_e2 *= rapport ;
364  v += proj_e2 ;
365  }
366 
368 
369  // Update the simulator frames
370  sim.set_fMo( wMo ); // This line is not really requested since the object doesn't move
371  sim.setCameraPositionRelObj( cMo );
372 
373  if (opt_display)
374  {
375  //Get the internal and external views
376  sim.getInternalImage(Iint);
377  sim.getExternalImage(Iext);
378 
379  //Display the object frame (current and desired position)
380  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
381  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
382 
383  //Display the object frame the world reference frame and the camera frame
387 
388  vpDisplay::flush(Iint);
389  vpDisplay::flush(Iext);
390  }
391 
392  vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
393 
394  std::cout << "|| s - s* || = " << (task.getError() ).sumSquare() <<std::endl ;
395  }
396 
397  task.print() ;
398  task.kill() ;
399 
400  return 0;
401  }
402  catch(vpException e) {
403  std::cout << "Catch an exception: " << e << std::endl;
404  return 1;
405  }
406 }
407 #else
408 int
409 main()
410 {
411  vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
412 }
413 
414 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
A cylinder of 80cm length and 10cm radius.
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
void set_fMo(const vpHomogeneousMatrix &fMo_)
#define vpERROR_TRACE
Definition: vpDebug.h:391
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:459
Define the X11 console to display images.
Definition: vpDisplayX.h:148
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:446
static const vpColor none
Definition: vpColor.h:175
error that can be emited by ViSP classes.
Definition: vpException.h:73
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:76
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1458
virtual void setSamplingTime(const double &delta_t)
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:105
void kill()
Definition: vpServo.cpp:186
vpColVector getError() const
Definition: vpServo.h:271
vpHomogeneousMatrix get_fMo() const
vpColVector computeControlLaw()
Definition: vpServo.cpp:899
virtual void setWindowPosition(int winx, int winy)=0
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:390
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:141
vpHomogeneousMatrix getPosition() const
Implementation of a velocity twist matrix and operations on such kind of matrices.
void getExternalImage(vpImage< unsigned char > &I)
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:519
static double rad(double deg)
Definition: vpMath.h:104
void setExternalCameraParameters(const vpCameraParameters &cam)
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, vpImagePoint offset=vpImagePoint(0, 0))
Definition: vpDisplay.cpp:373
void getInternalImage(vpImage< unsigned char > &I)
Class that defines what is a cylinder.
Definition: vpCylinder.h:93
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:434
vpHomogeneousMatrix inverse() const
void setInternalCameraParameters(const vpCameraParameters &cam)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:248
virtual bool getClick(bool blocking=true)=0
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:217
vpHomogeneousMatrix getExternalCameraPosition() const