Visual Servoing Platform  version 3.5.1 under development (2022-12-02)
servoSimuCylinder2DCamVelocityDisplaySecondaryTask.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 2D visual servoing on a cylinder.
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
54 #include <iostream>
55 #include <stdio.h>
56 #include <stdlib.h>
57 
58 #include <visp3/core/vpCameraParameters.h>
59 #include <visp3/core/vpCylinder.h>
60 #include <visp3/core/vpHomogeneousMatrix.h>
61 #include <visp3/core/vpImage.h>
62 #include <visp3/core/vpMath.h>
63 #include <visp3/gui/vpDisplayD3D.h>
64 #include <visp3/gui/vpDisplayGDI.h>
65 #include <visp3/gui/vpDisplayGTK.h>
66 #include <visp3/gui/vpDisplayOpenCV.h>
67 #include <visp3/gui/vpDisplayX.h>
68 #include <visp3/gui/vpProjectionDisplay.h>
69 #include <visp3/io/vpParseArgv.h>
70 #include <visp3/robot/vpSimulatorCamera.h>
71 #include <visp3/visual_features/vpFeatureBuilder.h>
72 #include <visp3/visual_features/vpFeatureLine.h>
73 #include <visp3/vs/vpServo.h>
74 #include <visp3/vs/vpServoDisplay.h>
75 
76 // List of allowed command line options
77 #define GETOPTARGS "cdho"
78 
79 void usage(const char *name, const char *badparam);
80 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
81 
90 void usage(const char *name, const char *badparam)
91 {
92  fprintf(stdout, "\n\
93 Simulation of a 2D visual servoing on a cylinder:\n\
94 - eye-in-hand control law,\n\
95 - velocity computed in the camera frame,\n\
96 - display the camera view.\n\
97  \n\
98 SYNOPSIS\n\
99  %s [-c] [-d] [-o] [-h]\n",
100  name);
101 
102  fprintf(stdout, "\n\
103 OPTIONS: Default\n\
104  \n\
105  -c\n\
106  Disable the mouse click. Useful to automaze the \n\
107  execution of this program without humain intervention.\n\
108  \n\
109  -d \n\
110  Turn off the display.\n\
111  \n\
112  -o \n\
113  Disable new projection operator usage for secondary task.\n\
114  \n\
115  -h\n\
116  Print the help.\n");
117 
118  if (badparam)
119  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
120 }
121 
134 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display, bool &new_proj_operator)
135 {
136  const char *optarg_;
137  int c;
138  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
139 
140  switch (c) {
141  case 'c':
142  click_allowed = false;
143  break;
144  case 'd':
145  display = false;
146  break;
147  case 'o':
148  new_proj_operator = false;
149  break;
150  case 'h':
151  usage(argv[0], NULL);
152  return false;
153 
154  default:
155  usage(argv[0], optarg_);
156  return false;
157  }
158  }
159 
160  if ((c == 1) || (c == -1)) {
161  // standalone param or error
162  usage(argv[0], NULL);
163  std::cerr << "ERROR: " << std::endl;
164  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
165  return false;
166  }
167 
168  return true;
169 }
170 
171 int main(int argc, const char **argv)
172 {
173 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
174  try {
175  bool opt_display = true;
176  bool opt_click_allowed = true;
177  bool opt_new_proj_operator = true;
178 
179  // Read the command line options
180  if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) == false) {
181  exit(-1);
182  }
183 
184  vpImage<unsigned char> Iint(512, 512, 0);
185  vpImage<unsigned char> Iext(512, 512, 0);
186 
187 // We open a window if a display is available
188 #ifdef VISP_HAVE_DISPLAY
189 #if defined VISP_HAVE_X11
190  vpDisplayX displayInt;
191  vpDisplayX displayExt;
192 #elif defined VISP_HAVE_GTK
193  vpDisplayGTK displayInt;
194  vpDisplayGTK displayExt;
195 #elif defined VISP_HAVE_GDI
196  vpDisplayGDI displayInt;
197  vpDisplayGDI displayExt;
198 #elif defined VISP_HAVE_OPENCV
199  vpDisplayOpenCV displayInt;
200  vpDisplayOpenCV displayExt;
201 #elif defined VISP_HAVE_D3D9
202  vpDisplayD3D displayInt;
203  vpDisplayD3D displayExt;
204 #endif
205 #endif
206 
207  if (opt_display) {
208 #ifdef VISP_HAVE_DISPLAY
209  // Display size is automatically defined by the image (Iint) and
210  // (Iext) size
211  displayInt.init(Iint, 100, 100, "Internal view");
212  displayExt.init(Iext, 130 + static_cast<int>(Iint.getWidth()), 100, "External view");
213 #endif
214  // Display the image
215  // The image class has a member that specify a pointer toward
216  // the display that has been initialized in the display declaration
217  // therefore is is no longuer necessary to make a reference to the
218  // display variable.
219  vpDisplay::display(Iint);
220  vpDisplay::display(Iext);
221  vpDisplay::flush(Iint);
222  vpDisplay::flush(Iext);
223  }
224 
225 #ifdef VISP_HAVE_DISPLAY
226  vpProjectionDisplay externalview;
227 #endif
228 
229  // Set the camera parameters
230  double px, py;
231  px = py = 600;
232  double u0, v0;
233  u0 = v0 = 256;
234 
235  vpCameraParameters cam(px, py, u0, v0);
236 
237  vpServo task;
238  vpSimulatorCamera robot;
239 
240  // sets the initial camera location
241  vpHomogeneousMatrix cMo(-0.2, 0.1, 2, vpMath::rad(5), vpMath::rad(5), vpMath::rad(20));
242 
243  vpHomogeneousMatrix wMc, wMo;
244  robot.getPosition(wMc);
245  wMo = wMc * cMo; // Compute the position of the object in the world frame
246 
247  // sets the final camera location (for simulation purpose)
248  vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
249 
250  // sets the cylinder coordinates in the world frame
251  vpCylinder cylinder(0, 1, 0, // direction
252  0, 0, 0, // point of the axis
253  0.1); // radius
254 
255 #ifdef VISP_HAVE_DISPLAY
256  externalview.insert(cylinder);
257 #endif
258  // sets the desired position of the visual feature
259  cylinder.track(cMod);
260  cylinder.print();
261 
262  // Build the desired line features thanks to the cylinder and especially
263  // its paramaters in the image frame
264  vpFeatureLine ld[2];
265  for (unsigned int i = 0; i < 2; i++)
266  vpFeatureBuilder::create(ld[i], cylinder, i);
267 
268  // computes the cylinder coordinates in the camera frame and its 2D
269  // coordinates sets the current position of the visual feature
270  cylinder.track(cMo);
271  cylinder.print();
272 
273  // Build the current line features thanks to the cylinder and especially
274  // its paramaters in the image frame
275  vpFeatureLine l[2];
276  for (unsigned int i = 0; i < 2; i++) {
277  vpFeatureBuilder::create(l[i], cylinder, i);
278  l[i].print();
279  }
280 
281  // define the task
282  // - we want an eye-in-hand control law
283  // - robot is controlled in the camera frame
286  // it can also be interesting to test these possibilities
287  // task.setInteractionMatrixType(vpServo::CURRENT,vpServo::PSEUDO_INVERSE)
288  // ; task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE)
289  // ; task.setInteractionMatrixType(vpServo::CURRENT,
290  // vpServo::PSEUDO_INVERSE) ;
291  // task.setInteractionMatrixType(vpServo::DESIRED, vpServo::TRANSPOSE) ;
292  // task.setInteractionMatrixType(vpServo::CURRENT, vpServo::TRANSPOSE) ;
293 
294  // we want to see 2 lines on 2 lines
295  task.addFeature(l[0], ld[0]);
296  task.addFeature(l[1], ld[1]);
297 
298  // Set the point of view of the external view
299  vpHomogeneousMatrix cextMo(0, 0, 6, vpMath::rad(40), vpMath::rad(10), vpMath::rad(60));
300 
301  // Display the initial scene
302  vpServoDisplay::display(task, cam, Iint);
303 #ifdef VISP_HAVE_DISPLAY
304  externalview.display(Iext, cextMo, cMo, cam, vpColor::red);
305 #endif
306  vpDisplay::flush(Iint);
307  vpDisplay::flush(Iext);
308 
309  // Display task information
310  task.print();
311 
312  if (opt_display && opt_click_allowed) {
313  vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo...", vpColor::white);
314  vpDisplay::flush(Iint);
315  vpDisplay::getClick(Iint);
316  }
317 
318  // set the gain
319  task.setLambda(1);
320 
321  // Display task information
322  task.print();
323 
324  unsigned int iter = 0;
325  bool stop = false;
326  bool start_secondary_task = false;
327 
328  while (!stop) {
329  std::cout << "---------------------------------------------" << iter++ << std::endl;
330 
331  // get the robot position
332  robot.getPosition(wMc);
333  // Compute the position of the object frame in the camera frame
334  cMo = wMc.inverse() * wMo;
335 
336  // new line position
337  // retrieve x,y and Z of the vpLine structure
338  // Compute the parameters of the cylinder in the camera frame and in the
339  // image frame
340  cylinder.track(cMo);
341 
342  // Build the current line features thanks to the cylinder and especially
343  // its paramaters in the image frame
344  for (unsigned int i = 0; i < 2; i++) {
345  vpFeatureBuilder::create(l[i], cylinder, i);
346  }
347 
348  // Display the current scene
349  if (opt_display) {
350  vpDisplay::display(Iint);
351  vpDisplay::display(Iext);
352  vpServoDisplay::display(task, cam, Iint);
353 #ifdef VISP_HAVE_DISPLAY
354  externalview.display(Iext, cextMo, cMo, cam, vpColor::red);
355 #endif
356  }
357 
358  // compute the control law
359  vpColVector v = task.computeControlLaw();
360 
361  // Wait primary task convergence before considering secondary task
362  if (task.getError().sumSquare() < 1e-6) {
363  start_secondary_task = true;
364  }
365 
366  if (start_secondary_task) {
367  // In this example the secondary task is cut in four
368  // steps. The first one consists in imposing a movement of the robot along
369  // the x axis of the object frame with a velocity of 0.5. The second one
370  // consists in imposing a movement of the robot along the y axis of the
371  // object frame with a velocity of 0.5. The third one consists in imposing a
372  // movement of the robot along the x axis of the object frame with a
373  // velocity of -0.5. The last one consists in imposing a movement of the
374  // robot along the y axis of the object frame with a velocity of -0.5.
375  // Each steps is made during 200 iterations.
376  vpColVector e1(6);
377  vpColVector e2(6);
378  vpColVector proj_e1;
379  vpColVector proj_e2;
380  static unsigned int iter_sec = 0;
381  double rapport = 0;
382  double vitesse = 0.5;
383  unsigned int tempo = 800;
384 
385  if (iter_sec > tempo) {
386  stop = true;
387  }
388 
389  if (iter_sec % tempo < 200) {
390  e2 = 0;
391  e1[0] = fabs(vitesse);
392  proj_e1 = task.secondaryTask(e1, opt_new_proj_operator);
393  rapport = vitesse / proj_e1[0];
394  proj_e1 *= rapport;
395  v += proj_e1;
396  }
397 
398  if (iter_sec % tempo < 400 && iter_sec % tempo >= 200) {
399  e1 = 0;
400  e2[1] = fabs(vitesse);
401  proj_e2 = task.secondaryTask(e2, opt_new_proj_operator);
402  rapport = vitesse / proj_e2[1];
403  proj_e2 *= rapport;
404  v += proj_e2;
405  }
406 
407  if (iter_sec % tempo < 600 && iter_sec % tempo >= 400) {
408  e2 = 0;
409  e1[0] = -fabs(vitesse);
410  proj_e1 = task.secondaryTask(e1, opt_new_proj_operator);
411  rapport = -vitesse / proj_e1[0];
412  proj_e1 *= rapport;
413  v += proj_e1;
414  }
415 
416  if (iter_sec % tempo < 800 && iter_sec % tempo >= 600) {
417  e1 = 0;
418  e2[1] = -fabs(vitesse);
419  proj_e2 = task.secondaryTask(e2, opt_new_proj_operator);
420  rapport = -vitesse / proj_e2[1];
421  proj_e2 *= rapport;
422  v += proj_e2;
423  }
424 
425  if (opt_display && opt_click_allowed) {
426  std::stringstream ss;
427  ss << std::string("New projection operator: ") +
428  (opt_new_proj_operator ? std::string("yes (use option -o to use old one)") : std::string("no"));
429  vpDisplay::displayText(Iint, 20, 20, "Secondary task enabled: yes", vpColor::white);
430  vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::white);
431  }
432 
433  iter_sec++;
434  } else {
435  if (opt_display && opt_click_allowed) {
436  vpDisplay::displayText(Iint, 20, 20, "Secondary task: no", vpColor::white);
437  }
438  }
439 
440  // send the camera velocity to the controller
442 
443  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
444 
445  if (opt_display) {
446  vpDisplay::displayText(Iint, 60, 20, "Click to stop visual servo...", vpColor::white);
447  if (vpDisplay::getClick(Iint, false)) {
448  stop = true;
449  }
450  vpDisplay::flush(Iint);
451  vpDisplay::flush(Iext);
452  }
453 
454  iter++;
455  }
456 
457  if (opt_display && opt_click_allowed) {
458  vpDisplay::display(Iint);
459  vpServoDisplay::display(task, cam, Iint);
460  vpDisplay::displayText(Iint, 20, 20, "Click to quit...", vpColor::white);
461  vpDisplay::flush(Iint);
462  vpDisplay::getClick(Iint);
463  }
464 
465  // Display task information
466  task.print();
467  return EXIT_SUCCESS;
468  } catch (const vpException &e) {
469  std::cout << "Catch a ViSP exception: " << e << std::endl;
470  return EXIT_FAILURE;
471  }
472 #else
473  (void)argc;
474  (void)argv;
475  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
476  return EXIT_SUCCESS;
477 #endif
478 }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
static const vpColor white
Definition: vpColor.h:212
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:103
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:107
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
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:135
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
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 ,...
void print(unsigned int select=FEATURE_ALL) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
Definition: vpMath.h:117
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
interface with the image for feature display
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
void insert(vpForwardProjection &fp)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition: vpRobot.h:83
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:564
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:303
void setLambda(double c)
Definition: vpServo.h:404
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1444
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:215
vpColVector getError() const
Definition: vpServo.h:278
@ PSEUDO_INVERSE
Definition: vpServo.h:202
vpColVector computeControlLaw()
Definition: vpServo.cpp:926
@ DESIRED
Definition: vpServo.h:186
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:487
Class that defines the simplest robot: a free flying camera.