Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuCylinder.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 * Demonstration of the wireframe simulator with a simple visual servoing
33 *
34*****************************************************************************/
35
42#include <stdlib.h>
43
44#include <visp3/core/vpCameraParameters.h>
45#include <visp3/core/vpCylinder.h>
46#include <visp3/core/vpHomogeneousMatrix.h>
47#include <visp3/core/vpImage.h>
48#include <visp3/core/vpIoTools.h>
49#include <visp3/core/vpMath.h>
50#include <visp3/core/vpTime.h>
51#include <visp3/core/vpVelocityTwistMatrix.h>
52#include <visp3/gui/vpDisplayD3D.h>
53#include <visp3/gui/vpDisplayGDI.h>
54#include <visp3/gui/vpDisplayGTK.h>
55#include <visp3/gui/vpDisplayOpenCV.h>
56#include <visp3/gui/vpDisplayX.h>
57#include <visp3/gui/vpPlot.h>
58#include <visp3/io/vpImageIo.h>
59#include <visp3/io/vpParseArgv.h>
60#include <visp3/robot/vpSimulatorCamera.h>
61#include <visp3/robot/vpWireFrameSimulator.h>
62#include <visp3/visual_features/vpFeatureBuilder.h>
63#include <visp3/vs/vpServo.h>
64
65#define GETOPTARGS "dhp"
66
67#if defined(VISP_HAVE_DISPLAY) && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
68
77void usage(const char *name, const char *badparam)
78{
79 fprintf(stdout, "\n\
80Demonstration of the wireframe simulator with a simple visual servoing.\n\
81 \n\
82The visual servoing consists in bringing the camera at a desired position\n\
83from the object.\n\
84 \n\
85The visual features used to compute the pose of the camera and \n\
86thus the control law are two lines. These features are computed thanks \n\
87to the equation of a cylinder.\n\
88 \n\
89This demonstration explains also how to move the object around a world \n\
90reference frame. Here, the movment is a rotation around the x and y axis \n\
91at a given distance from the world frame. In fact the object trajectory \n\
92is on a sphere whose center is the origin of the world frame.\n\
93 \n\
94SYNOPSIS\n\
95 %s [-d] [-p] [-h]\n",
96 name);
97
98 fprintf(stdout, "\n\
99OPTIONS: \n\
100 -d \n\
101 Turn off the display.\n\
102 \n\
103 -p \n\
104 Turn off the plotter.\n\
105 \n\
106 -h\n\
107 Print the help.\n");
108
109 if (badparam)
110 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
111}
112
125bool getOptions(int argc, const char **argv, bool &display, bool &plot)
126{
127 const char *optarg_;
128 int c;
129 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
130
131 switch (c) {
132 case 'd':
133 display = false;
134 break;
135 case 'p':
136 plot = 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_plot = true;
164
165 // Read the command line options
166 if (getOptions(argc, argv, opt_display, opt_plot) == false) {
167 return EXIT_FAILURE;
168 }
169
170 vpImage<vpRGBa> Iint(480, 640, 255);
171 vpImage<vpRGBa> Iext(480, 640, 255);
172
173#if defined(VISP_HAVE_X11)
174 vpDisplayX display[2];
175#elif defined(HAVE_OPENCV_HIGHGUI)
176 vpDisplayOpenCV display[2];
177#elif defined(VISP_HAVE_GDI)
178 vpDisplayGDI display[2];
179#elif defined(VISP_HAVE_D3D9)
180 vpDisplayD3D display[2];
181#elif defined(VISP_HAVE_GTK)
182 vpDisplayGTK display[2];
183#endif
184
185 if (opt_display) {
186 // Display size is automatically defined by the image (I) size
187 display[0].init(Iint, 100, 100, "The internal view");
188 display[1].init(Iext, 100, 100, "The first external view");
190 vpDisplay::setWindowPosition(Iext, 750, 0);
191 vpDisplay::display(Iint);
192 vpDisplay::flush(Iint);
193 vpDisplay::display(Iext);
194 vpDisplay::flush(Iext);
195 }
196
197 vpPlot *plotter = NULL;
198
199 vpServo task;
200 vpSimulatorCamera robot;
201 float sampling_time = 0.020f; // Sampling period in second
202 robot.setSamplingTime(sampling_time);
203
204 // Set initial position of the object in the camera frame
205 vpHomogeneousMatrix cMo(0, 0.1, 0.3, vpMath::rad(35), vpMath::rad(25), vpMath::rad(75));
206 // Set desired position of the object in the camera frame
207 vpHomogeneousMatrix cdMo(0.0, 0.0, 0.5, vpMath::rad(90), vpMath::rad(0), vpMath::rad(0));
208 // Set initial position of the object in the world frame
209 vpHomogeneousMatrix wMo(0.0, 0.0, 0, 0, 0, 0);
210 // Position of the camera in the world frame
212 wMc = wMo * cMo.inverse();
213
214 // Create a cylinder
215 vpCylinder cylinder(0, 0, 1, 0, 0, 0, 0.1);
216
217 // Projection of the cylinder
218 cylinder.track(cMo);
219
220 // Set the current visual feature
221 vpFeatureLine l[2];
224
225 // Projection of the cylinder
226 cylinder.track(cdMo);
227
228 vpFeatureLine ld[2];
231
234
236 vpVelocityTwistMatrix cVe(cMe);
237 task.set_cVe(cVe);
238
239 vpMatrix eJe;
240 robot.get_eJe(eJe);
241 task.set_eJe(eJe);
242
243 for (int i = 0; i < 2; i++)
244 task.addFeature(l[i], ld[i]);
245
246 if (opt_plot) {
247 plotter = new vpPlot(2, 480, 640, 750, 550, "Real time curves plotter");
248 plotter->setTitle(0, "Visual features error");
249 plotter->setTitle(1, "Camera velocities");
250 plotter->initGraph(0, task.getDimension());
251 plotter->initGraph(1, 6);
252 plotter->setLegend(0, 0, "error_feat_l1_rho");
253 plotter->setLegend(0, 1, "error_feat_l1_theta");
254 plotter->setLegend(0, 2, "error_feat_l2_rho");
255 plotter->setLegend(0, 3, "error_feat_l2_theta");
256 plotter->setLegend(1, 0, "vc_x");
257 plotter->setLegend(1, 1, "vc_y");
258 plotter->setLegend(1, 2, "vc_z");
259 plotter->setLegend(1, 3, "wc_x");
260 plotter->setLegend(1, 4, "wc_y");
261 plotter->setLegend(1, 5, "wc_z");
262 }
263
264 task.setLambda(1);
265
267
268 // Set the scene
270
271 // Initialize simulator frames
272 sim.set_fMo(wMo); // Position of the object in the world reference frame
273 sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
274 sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
275
276 // Set the External camera position
277 vpHomogeneousMatrix camMf(vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0));
278 sim.setExternalCameraPosition(camMf);
279
280 // Set the parameters of the cameras (internal and external)
281 vpCameraParameters camera(1000, 1000, 320, 240);
282 sim.setInternalCameraParameters(camera);
283 sim.setExternalCameraParameters(camera);
284
285 int max_iter = 10;
286
287 if (opt_display) {
288 max_iter = 2500;
289
290 // Get the internal and external views
291 sim.getInternalImage(Iint);
292 sim.getExternalImage(Iext);
293
294 // Display the object frame (current and desired position)
295 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
296 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
297
298 // Display the object frame the world reference frame and the camera
299 // frame
300 vpDisplay::displayFrame(Iext, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
301 vpDisplay::displayFrame(Iext, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
302 vpDisplay::displayFrame(Iext, camMf, camera, 0.2, vpColor::none);
303
304 vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo", vpColor::red);
305
306 vpDisplay::flush(Iint);
307 vpDisplay::flush(Iext);
308
309 std::cout << "Click on a display" << std::endl;
310 while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext, false)) {
311 };
312 }
313
314 robot.setPosition(wMc);
315
316 // Print the task
317 task.print();
318
319 int iter = 0;
320 bool stop = false;
321 vpColVector v;
322
323 // Set the secondary task parameters
324 vpColVector e1(6, 0);
325 vpColVector e2(6, 0);
326 vpColVector proj_e1;
327 vpColVector proj_e2;
328 double rapport = 0;
329 double vitesse = 0.3;
330 int tempo = 600;
331
332 double t_prev, t = vpTime::measureTimeMs();
333
334 while (iter++ < max_iter && !stop) {
335 t_prev = t;
337
338 if (opt_display) {
339 vpDisplay::display(Iint);
340 vpDisplay::display(Iext);
341 }
342
343 robot.get_eJe(eJe);
344 task.set_eJe(eJe);
345
346 wMc = robot.getPosition();
347 cMo = wMc.inverse() * wMo;
348
349 cylinder.track(cMo);
352
353 v = task.computeControlLaw();
354
355 // Compute the velocity with the secondary task
356 if (iter % tempo < 200 && iter % tempo >= 0) {
357 e2 = 0;
358 e1[0] = -fabs(vitesse);
359 proj_e1 = task.secondaryTask(e1, true);
360 rapport = -vitesse / proj_e1[0];
361 proj_e1 *= rapport;
362 v += proj_e1;
363 }
364
365 else if (iter % tempo < 300 && iter % tempo >= 200) {
366 e1 = 0;
367 e2[1] = -fabs(vitesse);
368 proj_e2 = task.secondaryTask(e2, true);
369 rapport = -vitesse / proj_e2[1];
370 proj_e2 *= rapport;
371 v += proj_e2;
372 }
373
374 else if (iter % tempo < 500 && iter % tempo >= 300) {
375 e2 = 0;
376 e1[0] = -fabs(vitesse);
377 proj_e1 = task.secondaryTask(e1, true);
378 rapport = vitesse / proj_e1[0];
379 proj_e1 *= rapport;
380 v += proj_e1;
381 }
382
383 else if (iter % tempo < 600 && iter % tempo >= 500) {
384 e1 = 0;
385 e2[1] = -fabs(vitesse);
386 proj_e2 = task.secondaryTask(e2, true);
387 rapport = vitesse / proj_e2[1];
388 proj_e2 *= rapport;
389 v += proj_e2;
390 }
391
393
394 // Update the simulator frames
395 sim.set_fMo(wMo); // This line is not really requested since the object
396 // doesn't move
398
399 if (opt_plot) {
400 plotter->plot(0, iter, task.getError());
401 plotter->plot(1, iter, v);
402 }
403
404 if (opt_display) {
405 // Get the internal and external views
406 sim.getInternalImage(Iint);
407 sim.getExternalImage(Iext);
408
409 // Display the object frame (current and desired position)
410 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
411 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
412
413 // Display the object frame the world reference frame and the camera
414 // frame
415 vpDisplay::displayFrame(Iext, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
419
420 vpDisplay::displayText(Iint, 20, 20, "Click to stop visual servo", vpColor::red);
421
422 std::stringstream ss;
423 ss << "Loop time: " << t - t_prev << " ms";
424 vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::red);
425
426 if (vpDisplay::getClick(Iint, false)) {
427 stop = true;
428 }
429
430 vpDisplay::flush(Iext);
431 vpDisplay::flush(Iint);
432
433 vpTime::wait(t, sampling_time * 1000); // Wait ms
434 }
435
436 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
437 }
438
439 if (opt_plot && plotter != NULL) {
440 vpDisplay::display(Iint);
441 sim.getInternalImage(Iint);
442 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
443 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
444 vpDisplay::displayText(Iint, 20, 20, "Click to quit", vpColor::red);
445 if (vpDisplay::getClick(Iint)) {
446 stop = true;
447 }
448 vpDisplay::flush(Iint);
449
450 delete plotter;
451 }
452
453 task.print();
454
455 return EXIT_SUCCESS;
456 }
457 catch (const vpException &e) {
458 std::cout << "Catch an exception: " << e << std::endl;
459 return EXIT_FAILURE;
460 }
461}
462#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
463int main()
464{
465 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
466 return EXIT_SUCCESS;
467}
468#else
469int main()
470{
471 std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..."
472 << std::endl;
473 std::cout << "Tip if you are on a unix-like system:" << std::endl;
474 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
475 std::cout << "Tip if you are on a windows-like system:" << std::endl;
476 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
477 return EXIT_SUCCESS;
478}
479
480#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
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 Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
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 displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
static void setWindowPosition(const vpImage< unsigned char > &I, int winx, int winy)
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 ,...
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
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition vpPlot.h:113
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition vpPlot.cpp:202
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition vpPlot.cpp:545
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition vpPlot.cpp:269
void setTitle(unsigned int graphNum, const std::string &title)
Definition vpPlot.cpp:503
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ CAMERA_FRAME
Definition vpRobot.h:80
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_L_cVe_eJe
Definition vpServo.h:155
unsigned int getDimension() const
Return the task dimension.
Definition vpServo.cpp:550
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition vpServo.h:448
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 set_eJe(const vpMatrix &eJe_)
Definition vpServo.h:506
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition vpServo.cpp:1454
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector getError() const
Definition vpServo.h:276
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.
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpHomogeneousMatrix getExternalCameraPosition() const
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
void getInternalImage(vpImage< unsigned char > &I)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void set_fMo(const vpHomogeneousMatrix &fMo_)
vpHomogeneousMatrix get_fMo() const
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
void setInternalCameraParameters(const vpCameraParameters &cam)
void setExternalCameraParameters(const vpCameraParameters &cam)
@ CYLINDER
A cylinder of 80cm length and 10cm radius.
void getExternalImage(vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)