Visual Servoing Platform  version 3.0.1
simulateCircle2DCamVelocity.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  * Simulation of a visual servoing with visualization.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
53 #include <visp3/core/vpConfig.h>
54 #include <visp3/core/vpDebug.h>
55 
56 
57 #ifdef VISP_HAVE_COIN3D_AND_GUI
58 #include <visp3/core/vpImage.h>
59 #include <visp3/core/vpCameraParameters.h>
60 #include <visp3/core/vpTime.h>
61 #include <visp3/ar/vpSimulator.h>
62 #include <visp3/core/vpMath.h>
63 #include <visp3/core/vpHomogeneousMatrix.h>
64 #include <visp3/visual_features/vpFeatureEllipse.h>
65 #include <visp3/core/vpCircle.h>
66 #include <visp3/vs/vpServo.h>
67 #include <visp3/robot/vpSimulatorCamera.h>
68 #include <visp3/visual_features/vpFeatureBuilder.h>
69 #include <visp3/io/vpParseArgv.h>
70 #include <visp3/core/vpIoTools.h>
71 
72 #define GETOPTARGS "cdi:h"
73 #define SAVE 0
74 
84 void usage(const char *name, const char *badparam, std::string ipath)
85 {
86  fprintf(stdout, "\n\
87 Simulation Servo Circle\n\
88  \n\
89 SYNOPSIS\n\
90  %s [-i <input image path>] [-d] [-h]\n", name);
91 
92 
93  fprintf(stdout, "\n\
94 OPTIONS: Default\n\
95  -i <input image path> %s\n\
96  Set image input path.\n\
97  From this path read \"ViSP-images/iv/4points.iv\"\n\
98  cad model.\n\
99  Setting the VISP_INPUT_IMAGE_PATH environment\n\
100  variable produces the same behaviour than using\n\
101  this option.\n\
102  \n\
103  -d \n\
104  Disable the image display. This can be useful \n\
105  for automatic tests using crontab under Unix or \n\
106  using the task manager under Windows.\n\
107  \n\
108  -h\n\
109  Print the help.\n\n",
110  ipath.c_str());
111 
112  if (badparam)
113  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
114 }
115 
131 bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
132 {
133  const char *optarg;
134  int c;
135  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
136 
137  switch (c) {
138  case 'i': ipath = optarg; break;
139  case 'd': display = false; break;
140  case 'h': usage(argv[0], NULL, ipath); return false; break;
141 
142  default:
143  usage(argv[0], optarg, ipath); return false; break;
144  }
145  }
146 
147  if ((c == 1) || (c == -1)) {
148  // standalone param or error
149  usage(argv[0], NULL, ipath);
150  std::cerr << "ERROR: " << std::endl;
151  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
152  return false;
153  }
154 
155  return true;
156 }
157 
158 static
159 void *mainLoop (void *_simu)
160 {
161  vpSimulator *simu = (vpSimulator *)_simu ;
162  simu->initMainApplication() ;
163 
164  vpPoseVector vcMo ;
165 
166  vcMo[0] = 0.3 ;
167  vcMo[1] = 0.2 ;
168  vcMo[2] = 3 ;
169  vcMo[3] = 0 ;
170  vcMo[4] = vpMath::rad(45) ;
171  vcMo[5] = vpMath::rad(40) ;
172  vpHomogeneousMatrix cMo(vcMo);
173  vpHomogeneousMatrix wMo; // Set to identity
174  vpHomogeneousMatrix wMc; // Robot (=camera) location in the world frame
175 
176  vpHomogeneousMatrix cMod ;
177  cMod[0][3] = 0 ;
178  cMod[1][3] = 0 ;
179  cMod[2][3] = 1 ;
180 
181  int it =0 ;
182  unsigned int pos = 2 ;
183  while (pos!=0)
184  {
185  vpServo task ;
186  vpSimulatorCamera robot ;
187 
188  float sampling_time = 0.040f; // Sampling period in second
189  robot.setSamplingTime(sampling_time);
190  robot.setMaxTranslationVelocity(4.);
191 
192  // Sets the initial camera location
193  wMc = wMo * cMo.inverse();
194  robot.setPosition(wMc) ;
195  simu->setCameraPosition(cMo) ;
196 
197  if (pos==1) cMod[2][3] = 0.32 ;
198 
199  // Sets the circle coordinates in the world frame
200  vpCircle circle ;
201  circle.setWorldCoordinates(0,0,1,0,0,0,0.1) ;
202 
203  // Sets the desired position of the visual feature
204  vpFeatureEllipse pd ;
205  circle.track(cMod) ;
206  vpFeatureBuilder::create(pd,circle) ;
207 
208  // Project : computes the circle coordinates in the camera frame and its 2D coordinates
209  // Sets the current position of the visual feature
210  vpFeatureEllipse p ;
211  circle.track(cMo) ;
212  vpFeatureBuilder::create(p,circle) ;
213 
214  // Define the task
215  // We want an eye-in-hand control law
216  // Robot is controlled in the camera frame
219 
220  // We want to see a circle on a circle
221  std::cout << std::endl ;
222  task.addFeature(p,pd) ;
223 
224  // Set the gain
225  task.setLambda(1.0) ;
226 
227  // Display task information
228  task.print() ;
229 
230  vpTime::wait(1000); // Sleep 1s
231 
232  unsigned int iter=0 ;
233  // Visual servoing loop
234  unsigned int itermax ;
235  if (pos==2) itermax = 75 ; else itermax = 100 ;
236  while(iter++ < itermax)
237  {
238  double t = vpTime::measureTimeMs();
239 
240  if (iter==1) std::cout << "get the robot position" << std::endl;
241  wMc = robot.getPosition() ;
242  if (iter==1) std::cout << "new circle position" << std::endl;
243  //retrieve x,y and Z of the vpCircle structure
244 
245  cMo = wMc.inverse() * wMo;
246  circle.track(cMo) ;
247  vpFeatureBuilder::create(p,circle);
248 
249  if (iter==1) std::cout << "compute the control law" << std::endl;
250  vpColVector v = task.computeControlLaw() ;
251  if (iter==1) {
252  std::cout << "Task rank: " << task.getTaskRank() <<std::endl ;
253  std::cout << "send the camera velocity to the controller" << std::endl;
254  }
256 
257  simu->setCameraPosition(cMo) ;
258 
259  if(SAVE==1)
260  {
261  char name[FILENAME_MAX] ;
262  sprintf(name,"/tmp/image.%04d.external.png",it) ;
263  std::cout << "Save " << name << std::endl ;
264  simu->write(name) ;
265  sprintf(name,"/tmp/image.%04u.internal.png",iter) ;
266  std::cout << "Save " << name << std::endl ;
267  simu->write(name) ;
268  it++ ;
269  }
270  // std::cout << "\t\t || s - s* || "
271  // std::cout << ( task.getError() ).sumSquare() <<std::endl ; ;
272  vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
273 
274  }
275  pos-- ;
276  task.kill();
277  }
278 
279  simu->closeMainApplication() ;
280 
281  void *a=NULL ;
282  return a ;
283 }
284 
285 
286 int
287 main(int argc, const char ** argv)
288 {
289  try {
290  std::string env_ipath;
291  std::string opt_ipath;
292  std::string ipath;
293  std::string filename;
294  bool opt_display = true;
295 
296  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
297  env_ipath = vpIoTools::getViSPImagesDataPath();
298 
299  // Set the default input path
300  if (! env_ipath.empty())
301  ipath = env_ipath;
302 
303  // Read the command line options
304  if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
305  exit (-1);
306  }
307 
308  // Get the option values
309  if (!opt_ipath.empty())
310  ipath = opt_ipath;
311 
312  // Compare ipath and env_ipath. If they differ, we take into account
313  // the input path comming from the command line option
314  if (!opt_ipath.empty() && !env_ipath.empty()) {
315  if (ipath != env_ipath) {
316  std::cout << std::endl
317  << "WARNING: " << std::endl;
318  std::cout << " Since -i <visp image path=" << ipath << "> "
319  << " is different from VISP_INPUT_IMAGE_PATH=" << env_ipath << std::endl
320  << " we skip the environment variable." << std::endl;
321  }
322  }
323 
324  // Test if an input path is set
325  if (opt_ipath.empty() && env_ipath.empty()){
326  usage(argv[0], NULL, ipath);
327  std::cerr << std::endl
328  << "ERROR:" << std::endl;
329  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
330  << std::endl
331  << " environment variable to specify the location of the " << std::endl
332  << " image path where test images are located." << std::endl << std::endl;
333  exit(-1);
334  }
335 
336  vpCameraParameters cam ;
337  vpHomogeneousMatrix fMo ; fMo[2][3] = 0 ;
338 
339  if (opt_display) {
340 
341  vpSimulator simu ;
342  simu.initInternalViewer(300, 300) ;
343  simu.initExternalViewer(300, 300) ;
344 
345  vpTime::wait(1000) ;
346  simu.setZoomFactor(1.0f) ;
347  simu.addAbsoluteFrame() ;
348 
349  // Load the cad model
350  filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/circle.iv");
351  simu.load(filename.c_str(),fMo) ;
352 
353  simu.setInternalCameraParameters(cam) ;
354 
355  simu.initApplication(&mainLoop) ;
356  simu.mainLoop() ;
357 
358  }
359  return 0;
360  }
361  catch(vpException &e) {
362  std::cout << "Catch an exception: " << e << std::endl;
363  return 1;
364  }
365 }
366 
367 
368 #else
369 int
370 main()
371 { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
372 
373 }
374 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:157
unsigned int getTaskRank() const
Definition: vpServo.cpp:1849
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1157
void write(const char *fileName)
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:238
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:98
void closeMainApplication()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:512
error that can be emited by ViSP classes.
Definition: vpException.h:73
void addAbsoluteFrame(float zoom=1)
Add the representation of the absolute frame.
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix getPosition() const
virtual void mainLoop()
activate the mainloop
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:76
virtual void setSamplingTime(const double &delta_t)
void kill()
Definition: vpServo.cpp:191
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
Definition: vpServo.cpp:954
#define vpTRACE
Definition: vpDebug.h:414
static std::string createFilePath(const std::string &parent, const std::string child)
Definition: vpIoTools.cpp:1366
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:391
void load(const char *file_name)
load an iv file
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:585
static double rad(double deg)
Definition: vpMath.h:104
void initMainApplication()
perform some initialization in the main program thread
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:314
Class that defines 2D ellipse visual feature.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines what is a circle.
Definition: vpCircle.h:57
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222
void setZoomFactor(const float zoom)
set the size of the camera/frame
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:62