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