Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
simulateFourPoints2DCartesianCamVelocity.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Simulation of a visual servoing with visualization.
33  *
34  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
52 #include <visp3/core/vpConfig.h>
53 #include <visp3/core/vpDebug.h>
54 
55 #ifdef VISP_HAVE_COIN3D_AND_GUI
56 
57 #include <visp3/ar/vpSimulator.h>
58 #include <visp3/core/vpCameraParameters.h>
59 #include <visp3/core/vpHomogeneousMatrix.h>
60 #include <visp3/core/vpImage.h>
61 #include <visp3/core/vpIoTools.h>
62 #include <visp3/core/vpMath.h>
63 #include <visp3/core/vpTime.h>
64 #include <visp3/io/vpParseArgv.h>
65 #include <visp3/robot/vpSimulatorCamera.h>
66 #include <visp3/visual_features/vpFeatureBuilder.h>
67 #include <visp3/visual_features/vpFeaturePoint.h>
68 #include <visp3/vs/vpServo.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 \"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':
135  ipath = optarg;
136  break;
137  case 'd':
138  display = false;
139  break;
140  case 'h':
141  usage(argv[0], NULL, ipath);
142  return false;
143  break;
144 
145  default:
146  usage(argv[0], optarg, ipath);
147  return false;
148  break;
149  }
150  }
151 
152  if ((c == 1) || (c == -1)) {
153  // standalone param or error
154  usage(argv[0], NULL, ipath);
155  std::cerr << "ERROR: " << std::endl;
156  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
157  return false;
158  }
159 
160  return true;
161 }
162 
163 static void *mainLoop(void *_simu)
164 {
165  vpSimulator *simu = static_cast<vpSimulator *>(_simu);
166  simu->initMainApplication();
167 
168  vpServo task;
169  vpSimulatorCamera robot;
170 
171  float sampling_time = 0.040f; // Sampling period in second
172  robot.setSamplingTime(sampling_time);
173 
174  std::cout << std::endl;
175  std::cout << "-------------------------------------------------------" << std::endl;
176  std::cout << " Test program for vpServo " << std::endl;
177  std::cout << " Eye-in-hand task control, articular velocities are computed" << std::endl;
178  std::cout << " Simulation " << std::endl;
179  std::cout << " task : servo 4 points " << std::endl;
180  std::cout << "-------------------------------------------------------" << std::endl;
181  std::cout << std::endl;
182 
183  // Sets the initial camera location
184  vpPoseVector vcMo;
185 
186  vcMo[0] = 0.3;
187  vcMo[1] = 0.2;
188  vcMo[2] = 3;
189  vcMo[3] = 0;
190  vcMo[4] = vpMath::rad(0);
191  vcMo[5] = vpMath::rad(40);
192 
193  vpHomogeneousMatrix cMo(vcMo);
194  vpHomogeneousMatrix wMo; // Set to identity
195  vpHomogeneousMatrix wMc; // Camera location in world frame
196  wMc = wMo * cMo.inverse();
197  robot.setPosition(wMc);
198  simu->setCameraPosition(cMo);
199 
200  simu->getCameraPosition(cMo);
201  wMc = wMo * cMo.inverse();
202  robot.setPosition(wMc);
203  robot.setMaxTranslationVelocity(4.);
204 
205  vpCameraParameters cam;
206 
207  // Sets the point coordinates in the world frame
208  vpPoint point[4];
209  point[0].setWorldCoordinates(-0.1, -0.1, 0);
210  point[1].setWorldCoordinates(0.1, -0.1, 0);
211  point[2].setWorldCoordinates(0.1, 0.1, 0);
212  point[3].setWorldCoordinates(-0.1, 0.1, 0);
213 
214  // Project : computes the point coordinates in the camera frame and its 2D
215  // coordinates
216  for (int i = 0; i < 4; i++)
217  point[i].track(cMo);
218 
219  // Sets the desired position of the point
220  vpFeaturePoint p[4];
221  for (int i = 0; i < 4; i++)
222  vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
223 
224  // Sets the desired position of the point
225  vpFeaturePoint pd[4];
226 
227  pd[0].buildFrom(-0.1, -0.1, 1);
228  pd[1].buildFrom(0.1, -0.1, 1);
229  pd[2].buildFrom(0.1, 0.1, 1);
230  pd[3].buildFrom(-0.1, 0.1, 1);
231 
232  // Define the task
233  // We want an eye-in-hand control law
234  // Articular velocity are computed
237 
238  // Set the position of the end-effector frame in the camera frame as identity
240  vpVelocityTwistMatrix cVe(cMe);
241  task.set_cVe(cVe);
242 
243  // Set the Jacobian (expressed in the end-effector frame)
244  vpMatrix eJe;
245  robot.get_eJe(eJe);
246  task.set_eJe(eJe);
247 
248  // We want to see a point on a point
249  for (int i = 0; i < 4; i++)
250  task.addFeature(p[i], pd[i]);
251 
252  // Set the gain
253  task.setLambda(1.0);
254 
255  std::cout << "Display task information" << std::endl;
256  task.print();
257 
258  vpTime::wait(1000); // Sleep 1s to ensure that all the thread are initialized
259 
260  unsigned int iter = 0;
261  // visual servo loop
262  while (iter++ < 100) {
263  double t = vpTime::measureTimeMs();
264 
265  vpColVector v;
266 
267  robot.get_eJe(eJe);
268  task.set_eJe(eJe);
269 
270  wMc = robot.getPosition();
271  cMo = wMc.inverse() * wMo;
272  for (int i = 0; i < 4; i++) {
273  point[i].track(cMo);
274  vpFeatureBuilder::create(p[i], point[i]);
275  }
276 
277  v = task.computeControlLaw();
279 
280  simu->setCameraPosition(cMo);
281 
282  if (SAVE == 1) {
283  char name[FILENAME_MAX];
284  sprintf(name, "/tmp/image.%04u.external.png", iter);
285  std::cout << name << std::endl;
286  simu->write(name);
287  sprintf(name, "/tmp/image.%04u.internal.png", iter);
288  simu->write(name);
289  }
290 
291  vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
292  }
293  std::cout << "\nDisplay task information" << std::endl;
294  task.print();
295  task.kill();
296 
297  simu->closeMainApplication();
298 
299  void *a = NULL;
300  return a;
301 }
302 
303 int main(int argc, const char **argv)
304 {
305  try {
306  std::string env_ipath;
307  std::string opt_ipath;
308  std::string ipath;
309  std::string filename;
310  bool opt_display = true;
311 
312  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
313  // environment variable value
314  env_ipath = vpIoTools::getViSPImagesDataPath();
315 
316  // Set the default input path
317  if (!env_ipath.empty())
318  ipath = env_ipath;
319 
320  // Read the command line options
321  if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
322  exit(-1);
323  }
324 
325  // Get the option values
326  if (!opt_ipath.empty())
327  ipath = opt_ipath;
328 
329  // Compare ipath and env_ipath. If they differ, we take into account
330  // the input path comming from the command line option
331  if (!opt_ipath.empty() && !env_ipath.empty()) {
332  if (ipath != env_ipath) {
333  std::cout << std::endl << "WARNING: " << std::endl;
334  std::cout << " Since -i <visp image path=" << ipath << "> "
335  << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
336  << " we skip the environment variable." << std::endl;
337  }
338  }
339 
340  // Test if an input path is set
341  if (opt_ipath.empty() && env_ipath.empty()) {
342  usage(argv[0], NULL, ipath);
343  std::cerr << std::endl << "ERROR:" << std::endl;
344  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
345  << " environment variable to specify the location of the " << std::endl
346  << " image path where test images are located." << std::endl
347  << std::endl;
348  exit(-1);
349  }
350 
351  vpCameraParameters cam;
353  fMo[2][3] = 0;
354 
355  if (opt_display) {
356  vpSimulator simu;
357  simu.initInternalViewer(300, 300);
358  simu.initExternalViewer(300, 300);
359 
360  vpTime::wait(1000);
361  simu.setZoomFactor(1.0f);
362 
363  // Load the cad model
364  filename = vpIoTools::createFilePath(ipath, "iv/4points.iv");
365  simu.load(filename.c_str());
366 
367  simu.setInternalCameraParameters(cam);
368  simu.setExternalCameraParameters(cam);
369  simu.initApplication(&mainLoop);
370 
371  simu.mainLoop();
372  }
373  return EXIT_SUCCESS;
374  } catch (const vpException &e) {
375  std::cout << "Catch an exception: " << e << std::endl;
376  return EXIT_FAILURE;
377  }
378 }
379 
380 #else
381 int main()
382 {
383  std::cout << "You do not have Coin3D and SoQT or SoWin or SoXt functionalities enabled..." << std::endl;
384  std::cout << "Tip:" << std::endl;
385  std::cout << "- Install Coin3D and SoQT or SoWin or SoXt, configure ViSP again using cmake and build again this example" << std::endl;
386  return EXIT_SUCCESS;
387 }
388 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
void buildFrom(double x, double y, double Z)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1292
void write(const char *fileName)
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
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.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:497
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:99
void setZoomFactor(float zoom)
set the size of the camera/frame
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:508
void closeMainApplication()
error that can be emited by ViSP classes.
Definition: vpException.h:71
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:126
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Class that defines what is a point.
Definition: vpPoint.h:58
virtual void setSamplingTime(const double &delta_t)
void kill()
Definition: vpServo.cpp:192
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1537
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
void getCameraPosition(vpHomogeneousMatrix &_cMf)
get the camera position (from an homogeneous matrix)
Definition: vpSimulator.h:249
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:406
virtual void initInternalViewer(unsigned int nlig, unsigned int ncol)
initialize the camera view
void load(const char *file_name)
load an iv file
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:574
static double rad(double deg)
Definition: vpMath.h:108
void initMainApplication()
perform some initialization in the main program thread
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:450
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:313
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:239