Visual Servoing Platform  version 3.0.1
simulateFourPoints2DCartesianCamVelocity.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 
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.%04u.external.png",iter) ;
278  std::cout << name << std::endl ;
279  simu->write(name) ;
280  sprintf(name,"/tmp/image.%04u.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  bool opt_display = true;
304 
305  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
306  env_ipath = vpIoTools::getViSPImagesDataPath();
307 
308  // Set the default input path
309  if (! env_ipath.empty())
310  ipath = env_ipath;
311 
312  // Read the command line options
313  if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
314  exit (-1);
315  }
316 
317  // Get the option values
318  if (!opt_ipath.empty())
319  ipath = opt_ipath;
320 
321  // Compare ipath and env_ipath. If they differ, we take into account
322  // the input path comming from the command line option
323  if (!opt_ipath.empty() && !env_ipath.empty()) {
324  if (ipath != env_ipath) {
325  std::cout << std::endl
326  << "WARNING: " << std::endl;
327  std::cout << " Since -i <visp image path=" << ipath << "> "
328  << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
329  << " we skip the environment variable." << std::endl;
330  }
331  }
332 
333  // Test if an input path is set
334  if (opt_ipath.empty() && env_ipath.empty()){
335  usage(argv[0], NULL, ipath);
336  std::cerr << std::endl
337  << "ERROR:" << std::endl;
338  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
339  << std::endl
340  << " environment variable to specify the location of the " << std::endl
341  << " image path where test images are located." << std::endl << std::endl;
342  exit(-1);
343  }
344 
345  vpCameraParameters cam ;
346  vpHomogeneousMatrix fMo ; fMo[2][3] = 0 ;
347 
348  if (opt_display) {
349  vpSimulator simu ;
350  simu.initInternalViewer(300, 300) ;
351  simu.initExternalViewer(300, 300) ;
352 
353  vpTime::wait(1000) ;
354  simu.setZoomFactor(1.0f) ;
355 
356  // Load the cad model
357  filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
358  simu.load(filename.c_str()) ;
359 
360  simu.setInternalCameraParameters(cam) ;
361  simu.setExternalCameraParameters(cam) ;
362  simu.initApplication(&mainLoop) ;
363 
364  simu.mainLoop() ;
365  }
366  return 0;
367  }
368  catch(vpException &e) {
369  std::cout << "Catch an exception: " << e << std::endl;
370  return 1;
371  }
372 }
373 
374 #else
375 int
376 main()
377 { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
378 
379 }
380 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
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
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1157
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:460
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 track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
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
Class that defines what is a point.
Definition: vpPoint.h:59
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
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: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
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:585
static double rad(double deg)
Definition: vpMath.h:104
void buildFrom(const double x, const double y, const double Z)
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:435
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
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:222
void setZoomFactor(const float zoom)
set the size of the camera/frame