Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuCylinder2DCamVelocityDisplay.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*****************************************************************************/
35
46#include <visp3/core/vpConfig.h>
47#include <visp3/core/vpDebug.h>
48
49#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
50 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
51
52#include <stdio.h>
53#include <stdlib.h>
54
55#include <visp3/core/vpCameraParameters.h>
56#include <visp3/core/vpCylinder.h>
57#include <visp3/core/vpHomogeneousMatrix.h>
58#include <visp3/core/vpImage.h>
59#include <visp3/core/vpMath.h>
60#include <visp3/gui/vpDisplayGDI.h>
61#include <visp3/gui/vpDisplayGTK.h>
62#include <visp3/gui/vpDisplayOpenCV.h>
63#include <visp3/gui/vpDisplayX.h>
64#include <visp3/io/vpParseArgv.h>
65#include <visp3/robot/vpSimulatorCamera.h>
66#include <visp3/visual_features/vpFeatureBuilder.h>
67#include <visp3/visual_features/vpFeatureLine.h>
68#include <visp3/vs/vpServo.h>
69#include <visp3/vs/vpServoDisplay.h>
70
71// List of allowed command line options
72#define GETOPTARGS "cdh"
73
74void usage(const char *name, const char *badparam);
75bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
76
85void usage(const char *name, const char *badparam)
86{
87 fprintf(stdout, "\n\
88Simulation of a 2D visual servoing on a cylinder:\n\
89- eye-in-hand control law,\n\
90- velocity computed in the camera frame,\n\
91- display the camera view.\n\
92 \n\
93SYNOPSIS\n\
94 %s [-c] [-d] [-h]\n",
95 name);
96
97 fprintf(stdout, "\n\
98OPTIONS: Default\n\
99 \n\
100 -c\n\
101 Disable the mouse click. Useful to automate the \n\
102 execution of this program without human intervention.\n\
103 \n\
104 -d \n\
105 Turn off the display.\n\
106 \n\
107 -h\n\
108 Print the help.\n");
109
110 if (badparam)
111 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
112}
113
125bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
126{
127 const char *optarg_;
128 int c;
129 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
130
131 switch (c) {
132 case 'c':
133 click_allowed = false;
134 break;
135 case 'd':
136 display = false;
137 break;
138 case 'h':
139 usage(argv[0], NULL);
140 return false;
141
142 default:
143 usage(argv[0], optarg_);
144 return false;
145 }
146 }
147
148 if ((c == 1) || (c == -1)) {
149 // standalone param or error
150 usage(argv[0], NULL);
151 std::cerr << "ERROR: " << std::endl;
152 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
153 return false;
154 }
155
156 return true;
157}
158
159int main(int argc, const char **argv)
160{
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, 255);
171
172 // We open a window using either X11, GTK or GDI.
173#if defined(VISP_HAVE_X11)
174 vpDisplayX display;
175#elif defined(VISP_HAVE_GTK)
176 vpDisplayGTK display;
177#elif defined(VISP_HAVE_GDI)
178 vpDisplayGDI display;
179#elif defined(HAVE_OPENCV_HIGHGUI)
180 vpDisplayOpenCV display;
181#endif
182
183 if (opt_display) {
184 try {
185 // Display size is automatically defined by the image (I) size
186 display.init(I, 100, 100, "Camera view...");
187 // Display the image
188 // The image class has a member that specify a pointer toward
189 // the display that has been initialized in the display declaration
190 // therefore is is no longer necessary to make a reference to the
191 // display variable.
194 }
195 catch (...) {
196 vpERROR_TRACE("Error while displaying the image");
197 return EXIT_FAILURE;
198 }
199 }
200
201 double px, py;
202 px = py = 600;
203 double u0, v0;
204 u0 = v0 = 256;
205
206 vpCameraParameters cam(px, py, u0, v0);
207
208 vpServo task;
209 vpSimulatorCamera robot;
210
211 // sets the initial camera location
212 vpHomogeneousMatrix cMo(-0.2, 0.1, 2, vpMath::rad(5), vpMath::rad(5), vpMath::rad(20));
213
214 vpHomogeneousMatrix wMc, wMo;
215 robot.getPosition(wMc);
216 wMo = wMc * cMo; // Compute the position of the object in the world frame
217
218 // sets the final camera location (for simulation purpose)
219 vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(-60), vpMath::rad(0), vpMath::rad(0));
220
221 // sets the cylinder coordinates in the world frame
222 vpCylinder cylinder(0, 1, 0, // direction
223 0, 0, 0, // point of the axis
224 0.1); // radius
225
226 // sets the desired position of the visual feature
227 cylinder.track(cMod);
228 cylinder.print();
229
230 vpFeatureLine ld[2];
231 for (unsigned int i = 0; i < 2; i++)
232 vpFeatureBuilder::create(ld[i], cylinder, i);
233
234 // computes the cylinder coordinates in the camera frame and its 2D
235 // coordinates sets the current position of the visual feature
236 cylinder.track(cMo);
237 cylinder.print();
238
239 vpFeatureLine l[2];
240 for (unsigned int i = 0; i < 2; i++) {
241 vpFeatureBuilder::create(l[i], cylinder, i);
242 l[i].print();
243 }
244
245 // define the task
246 // - we want an eye-in-hand control law
247 // - robot is controlled in the camera frame
249 // task.setInteractionMatrixType(vpServo::CURRENT,
250 // vpServo::PSEUDO_INVERSE) ;
251 // it can also be interesting to test these possibilities
252 // task.setInteractionMatrixType(vpServo::MEAN,
253 // vpServo::PSEUDO_INVERSE) ;
255 // task.setInteractionMatrixType(vpServo::DESIRED, vpServo::TRANSPOSE) ;
256 // task.setInteractionMatrixType(vpServo::CURRENT, vpServo::TRANSPOSE) ;
257
258 // - we want to see 2 lines on 2 lines
259 task.addFeature(l[0], ld[0]);
260 task.addFeature(l[1], ld[1]);
261
262 vpServoDisplay::display(task, cam, I);
264
265 // Display task information
266 task.print();
267
268 if (opt_display && opt_click_allowed) {
269 std::cout << "\n\nClick in the camera view window to start..." << std::endl;
271 }
272
273 // - set the gain
274 task.setLambda(1);
275
276 // Display task information
277 task.print();
278
279 unsigned int iter = 0;
280 // loop
281 do {
282 std::cout << "---------------------------------------------" << iter++ << std::endl;
283 vpColVector v;
284
285 // get the robot position
286 robot.getPosition(wMc);
287 // Compute the position of the object frame in the camera frame
288 cMo = wMc.inverse() * wMo;
289
290 // new line position
291 // retrieve x,y and Z of the vpLine structure
292 cylinder.track(cMo);
293 // cylinder.print() ;
294 for (unsigned int i = 0; i < 2; i++) {
295 vpFeatureBuilder::create(l[i], cylinder, i);
296 // l[i].print() ;
297 }
298
299 if (opt_display) {
301 vpServoDisplay::display(task, cam, I);
303 }
304
305 // compute the control law
306 v = task.computeControlLaw();
307
308 // send the camera velocity to the controller
310
311 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
312
313 // vpDisplay::getClick(I) ;
314 } while ((task.getError()).sumSquare() > 1e-9);
315
316 if (opt_display && opt_click_allowed) {
317 vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::black);
320 }
321
322 // Display task information
323 task.print();
324 return EXIT_SUCCESS;
325 }
326 catch (const vpException &e) {
327 std::cout << "Catch a ViSP exception: " << e << std::endl;
328 return EXIT_FAILURE;
329 }
330}
331
332#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
333int main()
334{
335 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
336 return EXIT_SUCCESS;
337}
338#else
339int main()
340{
341 std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) or OpenCV functionalities to display "
342 "images..."
343 << std::endl;
344 std::cout << "Tip if you are on a unix-like system:" << std::endl;
345 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
346 std::cout << "Tip if you are on a windows-like system:" << std::endl;
347 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
348 return EXIT_SUCCESS;
349}
350#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor black
Definition vpColor.h:205
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition vpCylinder.h:98
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
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:132
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:59
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
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double rad(double deg)
Definition vpMath.h:116
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition vpRobot.h:80
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:564
@ EYEINHAND_CAMERA
Definition vpServo.h:151
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition vpServo.cpp:299
void setLambda(double c)
Definition vpServo.h:403
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector getError() const
Definition vpServo.h:276
@ PSEUDO_INVERSE
Definition vpServo.h:199
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ DESIRED
Definition vpServo.h:183
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Class that defines the simplest robot: a free flying camera.
#define vpERROR_TRACE
Definition vpDebug.h:388