Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
servoSimuCylinder.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  * Demonstration of the wireframe simulator with a simple visual servoing
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
45 #include <stdlib.h>
46 
47 #include <visp3/core/vpCameraParameters.h>
48 #include <visp3/core/vpCylinder.h>
49 #include <visp3/core/vpHomogeneousMatrix.h>
50 #include <visp3/core/vpImage.h>
51 #include <visp3/core/vpIoTools.h>
52 #include <visp3/core/vpMath.h>
53 #include <visp3/core/vpTime.h>
54 #include <visp3/core/vpVelocityTwistMatrix.h>
55 #include <visp3/gui/vpDisplayD3D.h>
56 #include <visp3/gui/vpDisplayGDI.h>
57 #include <visp3/gui/vpDisplayGTK.h>
58 #include <visp3/gui/vpDisplayOpenCV.h>
59 #include <visp3/gui/vpDisplayX.h>
60 #include <visp3/gui/vpPlot.h>
61 #include <visp3/io/vpImageIo.h>
62 #include <visp3/io/vpParseArgv.h>
63 #include <visp3/robot/vpSimulatorCamera.h>
64 #include <visp3/robot/vpWireFrameSimulator.h>
65 #include <visp3/visual_features/vpFeatureBuilder.h>
66 #include <visp3/vs/vpServo.h>
67 
68 #define GETOPTARGS "dhp"
69 
70 #if defined(VISP_HAVE_DISPLAY) \
71  && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
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] [-p] [-h]\n", name);
100 
101  fprintf(stdout, "\n\
102 OPTIONS: \n\
103  -d \n\
104  Turn off the display.\n\
105  \n\
106  -p \n\
107  Turn off the plotter.\n\
108  \n\
109  -h\n\
110  Print the help.\n");
111 
112  if (badparam)
113  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
114 }
115 
128 bool getOptions(int argc, const char **argv, bool &display, bool &plot)
129 {
130  const char *optarg_;
131  int c;
132  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
133 
134  switch (c) {
135  case 'd':
136  display = false;
137  break;
138  case 'p':
139  plot = false;
140  break;
141  case 'h':
142  usage(argv[0], NULL);
143  return false;
144 
145  default:
146  usage(argv[0], optarg_);
147  return false;
148  }
149  }
150 
151  if ((c == 1) || (c == -1)) {
152  // standalone param or error
153  usage(argv[0], NULL);
154  std::cerr << "ERROR: " << std::endl;
155  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
156  return false;
157  }
158 
159  return true;
160 }
161 
162 int main(int argc, const char **argv)
163 {
164  try {
165  bool opt_display = true;
166  bool opt_plot = true;
167 
168  // Read the command line options
169  if (getOptions(argc, argv, opt_display, opt_plot) == false) {
170  exit(-1);
171  }
172 
173  vpImage<vpRGBa> Iint(480, 640, 255);
174  vpImage<vpRGBa> Iext(480, 640, 255);
175 
176 #if defined VISP_HAVE_X11
177  vpDisplayX display[2];
178 #elif defined VISP_HAVE_OPENCV
179  vpDisplayOpenCV display[2];
180 #elif defined VISP_HAVE_GDI
181  vpDisplayGDI display[2];
182 #elif defined VISP_HAVE_D3D9
183  vpDisplayD3D display[2];
184 #elif defined VISP_HAVE_GTK
185  vpDisplayGTK display[2];
186 #endif
187 
188  if (opt_display) {
189  // Display size is automatically defined by the image (I) size
190  display[0].init(Iint, 100, 100, "The internal view");
191  display[1].init(Iext, 100, 100, "The first external view");
192  vpDisplay::setWindowPosition(Iint, 0, 0);
193  vpDisplay::setWindowPosition(Iext, 750, 0);
194  vpDisplay::display(Iint);
195  vpDisplay::flush(Iint);
196  vpDisplay::display(Iext);
197  vpDisplay::flush(Iext);
198  }
199 
200  vpPlot *plotter = NULL;
201 
202  vpServo task;
203  vpSimulatorCamera robot;
204  float sampling_time = 0.020f; // Sampling period in second
205  robot.setSamplingTime(sampling_time);
206 
207  // Set initial position of the object in the camera frame
208  vpHomogeneousMatrix cMo(0, 0.1, 0.3, vpMath::rad(35), vpMath::rad(25), vpMath::rad(75));
209  // Set desired position of the object in the camera frame
210  vpHomogeneousMatrix cdMo(0.0, 0.0, 0.5, vpMath::rad(90), vpMath::rad(0), vpMath::rad(0));
211  // Set initial position of the object in the world frame
212  vpHomogeneousMatrix wMo(0.0, 0.0, 0, 0, 0, 0);
213  // Position of the camera in the world frame
215  wMc = wMo * cMo.inverse();
216 
217  // Create a cylinder
218  vpCylinder cylinder(0, 0, 1, 0, 0, 0, 0.1);
219 
220  // Projection of the cylinder
221  cylinder.track(cMo);
222 
223  // Set the current visual feature
224  vpFeatureLine l[2];
227 
228  // Projection of the cylinder
229  cylinder.track(cdMo);
230 
231  vpFeatureLine ld[2];
234 
237 
239  vpVelocityTwistMatrix cVe(cMe);
240  task.set_cVe(cVe);
241 
242  vpMatrix eJe;
243  robot.get_eJe(eJe);
244  task.set_eJe(eJe);
245 
246  for (int i = 0; i < 2; i++)
247  task.addFeature(l[i], ld[i]);
248 
249  if (opt_plot) {
250  plotter = new vpPlot(2, 480, 640, 750, 550, "Real time curves plotter");
251  plotter->setTitle(0, "Visual features error");
252  plotter->setTitle(1, "Camera velocities");
253  plotter->initGraph(0, task.getDimension());
254  plotter->initGraph(1, 6);
255  plotter->setLegend(0, 0, "error_feat_l1_rho");
256  plotter->setLegend(0, 1, "error_feat_l1_theta");
257  plotter->setLegend(0, 2, "error_feat_l2_rho");
258  plotter->setLegend(0, 3, "error_feat_l2_theta");
259  plotter->setLegend(1, 0, "vc_x");
260  plotter->setLegend(1, 1, "vc_y");
261  plotter->setLegend(1, 2, "vc_z");
262  plotter->setLegend(1, 3, "wc_x");
263  plotter->setLegend(1, 4, "wc_y");
264  plotter->setLegend(1, 5, "wc_z");
265  }
266 
267  task.setLambda(1);
268 
270 
271  // Set the scene
273 
274  // Initialize simulator frames
275  sim.set_fMo(wMo); // Position of the object in the world reference frame
276  sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
277  sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
278 
279  // Set the External camera position
280  vpHomogeneousMatrix camMf(vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0));
281  sim.setExternalCameraPosition(camMf);
282 
283  // Set the parameters of the cameras (internal and external)
284  vpCameraParameters camera(1000, 1000, 320, 240);
285  sim.setInternalCameraParameters(camera);
286  sim.setExternalCameraParameters(camera);
287 
288  int max_iter = 10;
289 
290  if (opt_display) {
291  max_iter = 2500;
292 
293  // Get the internal and external views
294  sim.getInternalImage(Iint);
295  sim.getExternalImage(Iext);
296 
297  // Display the object frame (current and desired position)
298  vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
299  vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
300 
301  // Display the object frame the world reference frame and the camera
302  // frame
303  vpDisplay::displayFrame(Iext, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
304  vpDisplay::displayFrame(Iext, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
305  vpDisplay::displayFrame(Iext, camMf, camera, 0.2, vpColor::none);
306 
307  vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo", vpColor::red);
308 
309  vpDisplay::flush(Iint);
310  vpDisplay::flush(Iext);
311 
312  std::cout << "Click on a display" << std::endl;
313  while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext, false)) {
314  };
315  }
316 
317  robot.setPosition(wMc);
318 
319  // Print the task
320  task.print();
321 
322  int iter = 0;
323  bool stop = false;
324  vpColVector v;
325 
326  // Set the secondary task parameters
327  vpColVector e1(6, 0);
328  vpColVector e2(6, 0);
329  vpColVector proj_e1;
330  vpColVector proj_e2;
331  double rapport = 0;
332  double vitesse = 0.3;
333  int tempo = 600;
334 
335  double t_prev, t = vpTime::measureTimeMs();
336 
337  while (iter++ < max_iter && !stop) {
338  t_prev = t;
339  t = vpTime::measureTimeMs();
340 
341  if (opt_display) {
342  vpDisplay::display(Iint);
343  vpDisplay::display(Iext);
344  }
345 
346  robot.get_eJe(eJe);
347  task.set_eJe(eJe);
348 
349  wMc = robot.getPosition();
350  cMo = wMc.inverse() * wMo;
351 
352  cylinder.track(cMo);
355 
356  v = task.computeControlLaw();
357 
358  // Compute the velocity with the secondary task
359  if (iter % tempo < 200 && iter % tempo >= 0) {
360  e2 = 0;
361  e1[0] = -fabs(vitesse);
362  proj_e1 = task.secondaryTask(e1, true);
363  rapport = -vitesse / proj_e1[0];
364  proj_e1 *= rapport;
365  v += proj_e1;
366  }
367 
368  else if (iter % tempo < 300 && iter % tempo >= 200) {
369  e1 = 0;
370  e2[1] = -fabs(vitesse);
371  proj_e2 = task.secondaryTask(e2, true);
372  rapport = -vitesse / proj_e2[1];
373  proj_e2 *= rapport;
374  v += proj_e2;
375  }
376 
377  else if (iter % tempo < 500 && iter % tempo >= 300) {
378  e2 = 0;
379  e1[0] = -fabs(vitesse);
380  proj_e1 = task.secondaryTask(e1, true);
381  rapport = vitesse / proj_e1[0];
382  proj_e1 *= rapport;
383  v += proj_e1;
384  }
385 
386  else if (iter % tempo < 600 && iter % tempo >= 500) {
387  e1 = 0;
388  e2[1] = -fabs(vitesse);
389  proj_e2 = task.secondaryTask(e2, true);
390  rapport = vitesse / proj_e2[1];
391  proj_e2 *= rapport;
392  v += proj_e2;
393  }
394 
396 
397  // Update the simulator frames
398  sim.set_fMo(wMo); // This line is not really requested since the object
399  // doesn't move
400  sim.setCameraPositionRelObj(cMo);
401 
402  if (opt_plot) {
403  plotter->plot(0, iter, task.getError());
404  plotter->plot(1, iter, v);
405  }
406 
407  if (opt_display) {
408  // Get the internal and external views
409  sim.getInternalImage(Iint);
410  sim.getExternalImage(Iext);
411 
412  // Display the object frame (current and desired position)
413  vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
414  vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
415 
416  // Display the object frame the world reference frame and the camera
417  // frame
418  vpDisplay::displayFrame(Iext, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
419  vpColor::none);
420  vpDisplay::displayFrame(Iext, sim.getExternalCameraPosition() * sim.get_fMo(), camera, 0.2, vpColor::none);
422 
423  vpDisplay::displayText(Iint, 20, 20, "Click to stop visual servo", vpColor::red);
424 
425  std::stringstream ss;
426  ss << "Loop time: " << t - t_prev << " ms";
427  vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::red);
428 
429  if (vpDisplay::getClick(Iint, false)) {
430  stop = true;
431  }
432 
433  vpDisplay::flush(Iext);
434  vpDisplay::flush(Iint);
435 
436  vpTime::wait(t, sampling_time * 1000); // Wait ms
437  }
438 
439  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
440  }
441 
442  if (opt_plot && plotter != NULL) {
443  vpDisplay::display(Iint);
444  sim.getInternalImage(Iint);
445  vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
446  vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
447  vpDisplay::displayText(Iint, 20, 20, "Click to quit", vpColor::red);
448  if (vpDisplay::getClick(Iint)) {
449  stop = true;
450  }
451  vpDisplay::flush(Iint);
452 
453  delete plotter;
454  }
455 
456  task.print();
457 
458  return EXIT_SUCCESS;
459  } catch (const vpException &e) {
460  std::cout << "Catch an exception: " << e << std::endl;
461  return EXIT_FAILURE;
462  }
463 }
464 #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
465 int main()
466 {
467  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
468  return EXIT_SUCCESS;
469 }
470 #else
471 int main()
472 {
473  std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..." << std::endl;
474  std::cout << "Tip if you are on a unix-like system:" << std::endl;
475  std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
476  std::cout << "Tip if you are on a windows-like system:" << std::endl;
477  std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
478  return EXIT_SUCCESS;
479 }
480 
481 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
A cylinder of 80cm length and 10cm radius.
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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.
vpHomogeneousMatrix get_fMo() const
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
void set_fMo(const vpHomogeneousMatrix &fMo_)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:134
static const vpColor none
Definition: vpColor.h:229
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpHomogeneousMatrix inverse() const
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
vpHomogeneousMatrix getPosition() const
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
static const vpColor red
Definition: vpColor.h:217
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1446
virtual void setSamplingTime(const double &delta_t)
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
Definition: vpDisplayD3D.h:106
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
unsigned int getDimension() const
Return the task dimension.
Definition: vpServo.cpp:553
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
void setLambda(double c)
Definition: vpServo.h:404
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:134
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:286
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:567
static double rad(double deg)
Definition: vpMath.h:110
void setExternalCameraParameters(const vpCameraParameters &cam)
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:206
void getInternalImage(vpImage< unsigned char > &I)
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:102
static void setWindowPosition(const vpImage< unsigned char > &I, int winx, int winy)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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, const vpImagePoint &offset=vpImagePoint(0, 0))
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
void setInternalCameraParameters(const vpCameraParameters &cam)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:306
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:115
vpColVector getError() const
Definition: vpServo.h:278
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:218
vpHomogeneousMatrix getExternalCameraPosition() const