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