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