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