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