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