Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuSphere.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 <cmath> // std::fabs
43#include <limits> // numeric_limits
44#include <stdlib.h>
45
46#include <visp3/core/vpCameraParameters.h>
47#include <visp3/core/vpHomogeneousMatrix.h>
48#include <visp3/core/vpImage.h>
49#include <visp3/core/vpIoTools.h>
50#include <visp3/core/vpMath.h>
51#include <visp3/core/vpSphere.h>
52#include <visp3/core/vpTime.h>
53#include <visp3/core/vpVelocityTwistMatrix.h>
54#include <visp3/gui/vpDisplayD3D.h>
55#include <visp3/gui/vpDisplayGDI.h>
56#include <visp3/gui/vpDisplayGTK.h>
57#include <visp3/gui/vpDisplayOpenCV.h>
58#include <visp3/gui/vpDisplayX.h>
59#include <visp3/gui/vpPlot.h>
60#include <visp3/io/vpImageIo.h>
61#include <visp3/io/vpParseArgv.h>
62#include <visp3/robot/vpSimulatorCamera.h>
63#include <visp3/robot/vpWireFrameSimulator.h>
64#include <visp3/visual_features/vpFeatureBuilder.h>
65#include <visp3/visual_features/vpGenericFeature.h>
66#include <visp3/vs/vpServo.h>
67
68#define GETOPTARGS "dhp"
69
70#if defined(VISP_HAVE_DISPLAY) && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
71
80void usage(const char *name, const char *badparam)
81{
82 fprintf(stdout, "\n\
83Demonstration of the wireframe simulator with a simple visual servoing.\n\
84 \n\
85The visual servoing consists in bringing the camera at a desired position from the object.\n\
86 \n\
87The visual features used to compute the pose of the camera and thus the control law are special moments computed with the sphere's parameters.\n\
88 \n\
89SYNOPSIS\n\
90 %s [-d] [-p] [-h]\n",
91 name);
92
93 fprintf(stdout, "\n\
94OPTIONS: Default\n\
95 -d \n\
96 Turn off the display.\n\
97 \n\
98 -p \n\
99 Turn off the plotter.\n\
100 \n\
101 -h\n\
102 Print the help.\n");
103
104 if (badparam)
105 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
106}
107
120bool getOptions(int argc, const char **argv, bool &display, bool &plot)
121{
122 const char *optarg_;
123 int c;
124 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
125
126 switch (c) {
127 case 'd':
128 display = false;
129 break;
130 case 'p':
131 plot = false;
132 break;
133 case 'h':
134 usage(argv[0], NULL);
135 return false;
136
137 default:
138 usage(argv[0], optarg_);
139 return false;
140 }
141 }
142
143 if ((c == 1) || (c == -1)) {
144 // standalone param or error
145 usage(argv[0], NULL);
146 std::cerr << "ERROR: " << std::endl;
147 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
148 return false;
149 }
150
151 return true;
152}
153
154/*
155 Computes the virtual visual features corresponding to the sphere and stores
156 it in the generic feature.
157
158 The visual feature vector is computed thanks to the following formula : s =
159 {sx, sy, sz} sx = gx*h2/(sqrt(h2+1) sx = gy*h2/(sqrt(h2+1) sz = sqrt(h2+1)
160
161 with gx and gy the center of gravity of the ellipse,
162 with h2 = (gx²+gy²)/(4*n20*gy²+4*n02*gx²-8n11gxgy)
163 with n20,n02,n11 the second order centered moments of the sphere normalized by its area
164 (i.e., such that \f$n_{ij} = \mu_{ij}/a\f$ where \f$\mu_{ij}\f$ are the centered moments and a the area)
165*/
166void computeVisualFeatures(const vpSphere &sphere, vpGenericFeature &s)
167{
168 double gx = sphere.get_x();
169 double gy = sphere.get_y();
170 double n02 = sphere.get_n02();
171 double n20 = sphere.get_n20();
172 double n11 = sphere.get_n11();
173 double h2;
174 // if (gx != 0 || gy != 0)
175 if (std::fabs(gx) > std::numeric_limits<double>::epsilon() || std::fabs(gy) > std::numeric_limits<double>::epsilon())
176 h2 = (vpMath::sqr(gx) + vpMath::sqr(gy)) /
177 (4 * n20 * vpMath::sqr(gy) + 4 * n02 * vpMath::sqr(gx) - 8 * n11 * gx * gy);
178 else
179 h2 = 1 / (4 * n20);
180
181 double sx = gx * h2 / (sqrt(h2 + 1));
182 double sy = gy * h2 / (sqrt(h2 + 1));
183 double sz = sqrt(h2 + 1); //(h2-(vpMath::sqr(sx)+vpMath::sqr(sy)-1))/(2*sqrt(h2));
184
185 s.set_s(sx, sy, sz);
186}
187
188/*
189 Computes the interaction matrix such as L = [-1/R*I3 [s]x]
190
191 with R the radius of the sphere
192 with I3 the 3x3 identity matrix
193 with [s]x the skew matrix related to s
194*/
195void computeInteractionMatrix(const vpGenericFeature &s, const vpSphere &sphere, vpMatrix &L)
196{
197 L = 0;
198 L[0][0] = -1 / sphere.getR();
199 L[1][1] = -1 / sphere.getR();
200 L[2][2] = -1 / sphere.getR();
201
202 double s0, s1, s2;
203 s.get_s(s0, s1, s2);
204
205 vpTranslationVector c(s0, s1, s2);
206 vpMatrix sk;
207 sk = c.skew();
208
209 for (unsigned int i = 0; i < 3; i++)
210 for (unsigned int j = 0; j < 3; j++)
211 L[i][j + 3] = sk[i][j];
212}
213
214int main(int argc, const char **argv)
215{
216 try {
217 bool opt_display = true;
218 bool opt_plot = true;
219
220 // Read the command line options
221 if (getOptions(argc, argv, opt_display, opt_plot) == false) {
222 return EXIT_FAILURE;
223 }
224
225 vpImage<vpRGBa> Iint(480, 640, 255);
226 vpImage<vpRGBa> Iext1(480, 640, 255);
227 vpImage<vpRGBa> Iext2(480, 640, 255);
228
229#if defined(VISP_HAVE_X11)
230 vpDisplayX display[3];
231#elif defined(HAVE_OPENCV_HIGHGUI)
232 vpDisplayOpenCV display[3];
233#elif defined(VISP_HAVE_GDI)
234 vpDisplayGDI display[3];
235#elif defined(VISP_HAVE_D3D9)
236 vpDisplayD3D display[3];
237#elif defined(VISP_HAVE_GTK)
238 vpDisplayGTK display[3];
239#endif
240
241 if (opt_display) {
242 // Display size is automatically defined by the image (I) size
243 display[0].init(Iint, 100, 100, "The internal view");
244 display[1].init(Iext1, 100, 100, "The first external view");
245 display[2].init(Iext2, 100, 100, "The second external view");
247 vpDisplay::setWindowPosition(Iext1, 750, 0);
248 vpDisplay::setWindowPosition(Iext2, 0, 550);
249 vpDisplay::display(Iint);
250 vpDisplay::flush(Iint);
251 vpDisplay::display(Iext1);
252 vpDisplay::flush(Iext1);
253 vpDisplay::display(Iext2);
254 vpDisplay::flush(Iext2);
255 }
256
257 vpPlot *plotter = NULL;
258
259 vpServo task;
260 vpSimulatorCamera robot;
261 float sampling_time = 0.020f; // Sampling period in second
262 robot.setSamplingTime(sampling_time);
263
264 // Since the task gain lambda is very high, we need to increase default
265 // max velocities
268
269 // Set initial position of the object in the camera frame
270 vpHomogeneousMatrix cMo(0, 0.1, 2.0, vpMath::rad(35), vpMath::rad(25), 0);
271 // Set desired position of the object in the camera frame
272 vpHomogeneousMatrix cdMo(0.0, 0.0, 0.8, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
273 // Set initial position of the object in the world frame
274 vpHomogeneousMatrix wMo(0.0, 0.0, 0, 0, 0, 0);
275 // Position of the camera in the world frame
277 wMc = wMo * cMo.inverse();
278
279 robot.setPosition(wMc);
280
281 // The sphere
282 vpSphere sphere(0, 0, 0, 0.15);
283
284 // Projection of the sphere
285 sphere.track(cMo);
286
287 // Set the current visual feature
288 vpGenericFeature s(3);
289 computeVisualFeatures(sphere, s);
290
291 // Projection of the points
292 sphere.track(cdMo);
293
294 vpGenericFeature sd(3);
295 computeVisualFeatures(sphere, sd);
296
297 vpMatrix L(3, 6);
298 computeInteractionMatrix(sd, sphere, L);
299 sd.setInteractionMatrix(L);
300
303
305 vpVelocityTwistMatrix cVe(cMe);
306 task.set_cVe(cVe);
307
308 vpMatrix eJe;
309 robot.get_eJe(eJe);
310 task.set_eJe(eJe);
311
312 task.addFeature(s, sd);
313
314 task.setLambda(7);
315
316 if (opt_plot) {
317 plotter = new vpPlot(2, 480, 640, 750, 550, "Real time curves plotter");
318 plotter->setTitle(0, "Visual features error");
319 plotter->setTitle(1, "Camera velocities");
320 plotter->initGraph(0, task.getDimension());
321 plotter->initGraph(1, 6);
322 plotter->setLegend(0, 0, "error_feat_sx");
323 plotter->setLegend(0, 1, "error_feat_sy");
324 plotter->setLegend(0, 2, "error_feat_sz");
325 plotter->setLegend(1, 0, "vc_x");
326 plotter->setLegend(1, 1, "vc_y");
327 plotter->setLegend(1, 2, "vc_z");
328 plotter->setLegend(1, 3, "wc_x");
329 plotter->setLegend(1, 4, "wc_y");
330 plotter->setLegend(1, 5, "wc_z");
331 }
332
334
335 // Set the scene
337
338 // Initialize simulator frames
339 sim.set_fMo(wMo); // Position of the object in the world reference frame
340 sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
341 sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
342
343 // Set the External camera position
344 vpHomogeneousMatrix camMf(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0);
345 sim.setExternalCameraPosition(camMf);
346
347 // Computes the position of a camera which is fixed in the object frame
348 vpHomogeneousMatrix camoMf(0, 0.0, 2.5, 0, vpMath::rad(140), 0);
349 camoMf = camoMf * (sim.get_fMo().inverse());
350
351 // Set the parameters of the cameras (internal and external)
352 vpCameraParameters camera(1000, 1000, 320, 240);
353 sim.setInternalCameraParameters(camera);
354 sim.setExternalCameraParameters(camera);
355
356 int max_iter = 10;
357
358 if (opt_display) {
359 max_iter = 1000;
360 // Get the internal and external views
361 sim.getInternalImage(Iint);
362 sim.getExternalImage(Iext1);
363 sim.getExternalImage(Iext2, camoMf);
364
365 // Display the object frame (current and desired position)
366 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
367 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
368
369 // Display the object frame the world reference frame and the camera
370 // frame
371 vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
372 vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
373 vpDisplay::displayFrame(Iext1, camMf, camera, 0.2, vpColor::none);
374
375 // Display the world reference frame and the object frame
376 vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
377 vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
378
379 vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo", vpColor::red);
380
381 vpDisplay::flush(Iint);
382 vpDisplay::flush(Iext1);
383 vpDisplay::flush(Iext2);
384
385 std::cout << "Click on a display" << std::endl;
386 while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) &&
387 !vpDisplay::getClick(Iext2, false)) {
388 };
389 }
390
391 // Print the task
392 task.print();
393
394 int iter = 0;
395 bool stop = false;
396 vpColVector v;
397
398 double t_prev, t = vpTime::measureTimeMs();
399
400 while (iter++ < max_iter && !stop) {
401 t_prev = t;
403
404 if (opt_display) {
405 vpDisplay::display(Iint);
406 vpDisplay::display(Iext1);
407 vpDisplay::display(Iext2);
408 }
409
410 robot.get_eJe(eJe);
411 task.set_eJe(eJe);
412
413 wMc = robot.getPosition();
414 cMo = wMc.inverse() * wMo;
415
416 sphere.track(cMo);
417
418 // Set the current visual feature
419 computeVisualFeatures(sphere, s);
420
421 v = task.computeControlLaw();
424
425 // Compute the position of the external view which is fixed in the
426 // object frame
427 camoMf.buildFrom(0, 0.0, 2.5, 0, vpMath::rad(150), 0);
428 camoMf = camoMf * (sim.get_fMo().inverse());
429
430 if (opt_plot) {
431 plotter->plot(0, iter, task.getError());
432 plotter->plot(1, iter, v);
433 }
434
435 if (opt_display) {
436 // Get the internal and external views
437 sim.getInternalImage(Iint);
438 sim.getExternalImage(Iext1);
439 sim.getExternalImage(Iext2, camoMf);
440
441 // Display the object frame (current and desired position)
442 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
443 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
444
445 // Display the camera frame, the object frame the world reference
446 // frame
447 vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
451
452 // Display the world reference frame and the object frame
453 vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
454 vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
455
456 vpDisplay::displayText(Iint, 20, 20, "Click to stop visual servo", vpColor::red);
457
458 std::stringstream ss;
459 ss << "Loop time: " << t - t_prev << " ms";
460 vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::red);
461
462 if (vpDisplay::getClick(Iint, false)) {
463 stop = true;
464 }
465
466 vpDisplay::flush(Iint);
467 vpDisplay::flush(Iext1);
468 vpDisplay::flush(Iext2);
469
470 vpTime::wait(t, sampling_time * 1000); // Wait ms
471 }
472
473 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
474 }
475
476 if (opt_plot && plotter != NULL) {
477 vpDisplay::display(Iint);
478 sim.getInternalImage(Iint);
479 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
480 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
481 vpDisplay::displayText(Iint, 20, 20, "Click to quit", vpColor::red);
482 if (vpDisplay::getClick(Iint)) {
483 stop = true;
484 }
485 vpDisplay::flush(Iint);
486
487 delete plotter;
488 }
489
490 task.print();
491 return EXIT_SUCCESS;
492 }
493 catch (const vpException &e) {
494 std::cout << "Catch an exception: " << e << std::endl;
495 return EXIT_FAILURE;
496 }
497}
498#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
499int main()
500{
501 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
502 return EXIT_SUCCESS;
503}
504#else
505int main()
506{
507 std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..."
508 << std::endl;
509 std::cout << "Tip if you are on a unix-like system:" << std::endl;
510 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
511 std::cout << "Tip if you are on a windows-like system:" << std::endl;
512 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
513 return EXIT_SUCCESS;
514}
515#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
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
void track(const vpHomogeneousMatrix &cMo)
Class that enables to define a feature or a set of features which are not implemented in ViSP as a sp...
void get_s(vpColVector &s) const
get the value of all the features.
void set_s(const vpColVector &s)
set the value of all the features.
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 double sqr(double x)
Definition vpMath.h:124
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 setMaxRotationVelocity(double maxVr)
Definition vpRobot.cpp:257
void setMaxTranslationVelocity(double maxVt)
Definition vpRobot.cpp:236
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
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.
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition vpSphere.h:78
double get_n02() const
Definition vpSphere.h:104
double get_y() const
Definition vpSphere.h:100
double get_n11() const
Definition vpSphere.h:103
double get_x() const
Definition vpSphere.h:99
double getR() const
Definition vpSphere.h:109
double get_n20() const
Definition vpSphere.h:102
Class that consider the case of a translation vector.
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)
@ SPHERE
A 15cm radius sphere.
void getExternalImage(vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)