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