Visual Servoing Platform  version 3.6.1 under development (2025-02-18)
servoSimuSphere2DCamVelocityDisplay.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 sphere.
33  *
34 *****************************************************************************/
35 
45 #include <stdio.h>
46 #include <stdlib.h>
47 
48 #include <visp3/core/vpConfig.h>
49 #include <visp3/core/vpHomogeneousMatrix.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpSphere.h>
52 #include <visp3/gui/vpDisplayFactory.h>
53 #include <visp3/io/vpParseArgv.h>
54 #include <visp3/robot/vpSimulatorCamera.h>
55 #include <visp3/visual_features/vpFeatureBuilder.h>
56 #include <visp3/visual_features/vpFeatureEllipse.h>
57 #include <visp3/vs/vpServo.h>
58 #include <visp3/vs/vpServoDisplay.h>
59 
60 // List of allowed command line options
61 #define GETOPTARGS "cdh"
62 
63 #ifdef ENABLE_VISP_NAMESPACE
64 using namespace VISP_NAMESPACE_NAME;
65 #endif
66 
67 void usage(const char *name, const char *badparam);
68 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
69 
78 void usage(const char *name, const char *badparam)
79 {
80  fprintf(stdout, "\n\
81 Simulation of a 2D visual servoing on a sphere:\n\
82 - eye-in-hand control law,\n\
83 - velocity computed in the camera frame,\n\
84 - display the camera view.\n\
85  \n\
86 SYNOPSIS\n\
87  %s [-c] [-d] [-h]\n",
88  name);
89 
90  fprintf(stdout, "\n\
91 OPTIONS: Default\n\
92  \n\
93  -c\n\
94  Disable the mouse click. Useful to automate the \n\
95  execution of this program without human intervention.\n\
96  \n\
97  -d \n\
98  Turn off the display.\n\
99  \n\
100  -h\n\
101  Print the help.\n");
102 
103  if (badparam)
104  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
105 }
106 
119 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
120 {
121  const char *optarg_;
122  int c;
123  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
124 
125  switch (c) {
126  case 'c':
127  click_allowed = false;
128  break;
129  case 'd':
130  display = false;
131  break;
132  case 'h':
133  usage(argv[0], nullptr);
134  return false;
135 
136  default:
137  usage(argv[0], optarg_);
138  return false;
139  }
140  }
141 
142  if ((c == 1) || (c == -1)) {
143  // standalone param or error
144  usage(argv[0], nullptr);
145  std::cerr << "ERROR: " << std::endl;
146  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
147  return false;
148  }
149 
150  return true;
151 }
152 
153 int main(int argc, const char **argv)
154 {
155 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
156 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
157  std::shared_ptr<vpDisplay> display;
158 #else
159  vpDisplay *display = nullptr;
160 #endif
161  try {
162  bool opt_display = true;
163  bool opt_click_allowed = true;
164 
165  // Read the command line options
166  if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
167  return (EXIT_FAILURE);
168  }
169 
170  vpImage<unsigned char> I(512, 512, 0);
171 
172  if (opt_display) {
173 #if defined(VISP_HAVE_DISPLAY)
174  // Display size is automatically defined by the image (I) size
175 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
176  display = vpDisplayFactory::createDisplay(I, 100, 100, "Camera view...");
177 #else
178  display = vpDisplayFactory::allocateDisplay(I, 100, 100, "Camera view...");
179 #endif
180 #endif
181  // Display the image
182  // The image class has a member that specify a pointer toward
183  // the display that has been initialized in the display declaration
184  // therefore is is no longer necessary to make a reference to the
185  // display variable.
187  vpDisplay::flush(I);
188  }
189 
190  double px = 600, py = 600;
191  double u0 = I.getWidth() / 2., v0 = I.getHeight() / 2.;
192 
193  vpCameraParameters cam(px, py, u0, v0);
194 
195  vpServo task;
196  vpSimulatorCamera robot;
197 
198  // sets the initial camera location
200  cMo[0][3] = 0.1;
201  cMo[1][3] = 0.2;
202  cMo[2][3] = 2;
203  // Compute the position of the object in the world frame
204  vpHomogeneousMatrix wMc, wMo;
205  robot.getPosition(wMc);
206  wMo = wMc * cMo;
207 
208  vpHomogeneousMatrix cMod;
209  cMod[0][3] = 0;
210  cMod[1][3] = 0;
211  cMod[2][3] = 1;
212 
213  // sets the sphere coordinates in the world frame
214  vpSphere sphere;
215  sphere.setWorldCoordinates(0, 0, 0, 0.1);
216 
217  // sets the desired position of the visual feature
218  vpFeatureEllipse pd;
219  sphere.track(cMod);
220  vpFeatureBuilder::create(pd, sphere);
221 
222  // computes the sphere coordinates in the camera frame and its 2D
223  // coordinates sets the current position of the visual feature
225  sphere.track(cMo);
226  vpFeatureBuilder::create(p, sphere);
227 
228  // define the task
229  // - we want an eye-in-hand control law
230  // - robot is controlled in the camera frame
232 
233  // we want to see a sphere on a sphere
234  task.addFeature(p, pd);
235 
236  // set the gain
237  task.setLambda(1);
238 
239  // Display task information
240  task.print();
241 
242  unsigned int iter = 0;
243  // loop
244  while (iter++ < 200) {
245  std::cout << "---------------------------------------------" << iter << std::endl;
246  vpColVector v;
247 
248  // get the robot position
249  robot.getPosition(wMc);
250  // Compute the position of the object frame in the camera frame
251  cMo = wMc.inverse() * wMo;
252 
253  // new sphere position: retrieve x,y and Z of the vpSphere structure
254  sphere.track(cMo);
255  vpFeatureBuilder::create(p, sphere);
256 
257  if (opt_display) {
259  vpServoDisplay::display(task, cam, I);
260  vpDisplay::flush(I);
261  }
262 
263  // compute the control law
264  v = task.computeControlLaw();
265 
266  std::cout << "Task rank: " << task.getTaskRank() << std::endl;
267  // send the camera velocity to the controller
269 
270  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
271  }
272 
273  if (opt_display && opt_click_allowed) {
274  vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::white);
275  vpDisplay::flush(I);
277  }
278 
279  // Display task information
280  task.print();
281 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
282  if (display != nullptr) {
283  delete display;
284  }
285 #endif
286  return EXIT_SUCCESS;
287  }
288  catch (const vpException &e) {
289  std::cout << "Catch a ViSP exception: " << e << std::endl;
290 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
291  if (display != nullptr) {
292  delete display;
293  }
294 #endif
295  return EXIT_FAILURE;
296  }
297 #else
298  (void)argc;
299  (void)argv;
300  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
301  return EXIT_SUCCESS;
302 #endif
303  }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
static const vpColor white
Definition: vpColor.h:193
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 2D ellipse visual feature.
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:70
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)
@ EYEINHAND_CAMERA
Definition: vpServo.h:161
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:331
unsigned int getTaskRank() const
Definition: vpServo.h:611
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
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:134
vpColVector getError() const
Definition: vpServo.h:515
vpColVector computeControlLaw()
Definition: vpServo.cpp:705
Class that defines the simplest robot: a free flying camera.
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:80
void setWorldCoordinates(const vpColVector &oP) VP_OVERRIDE
Definition: vpSphere.cpp:61
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.