Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimu4Points.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/vpHomogeneousMatrix.h>
46#include <visp3/core/vpImage.h>
47#include <visp3/core/vpIoTools.h>
48#include <visp3/core/vpMath.h>
49#include <visp3/core/vpTime.h>
50#include <visp3/core/vpVelocityTwistMatrix.h>
51#include <visp3/gui/vpDisplayD3D.h>
52#include <visp3/gui/vpDisplayGDI.h>
53#include <visp3/gui/vpDisplayGTK.h>
54#include <visp3/gui/vpDisplayOpenCV.h>
55#include <visp3/gui/vpDisplayX.h>
56#include <visp3/gui/vpPlot.h>
57#include <visp3/io/vpImageIo.h>
58#include <visp3/io/vpParseArgv.h>
59#include <visp3/robot/vpSimulatorCamera.h>
60#include <visp3/robot/vpWireFrameSimulator.h>
61#include <visp3/visual_features/vpFeatureBuilder.h>
62#include <visp3/visual_features/vpFeaturePoint.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 \n\
83position from the object.\n\
84 \n\
85The visual features used to compute the pose of the camera and \n\
86thus the control law are four points.\n\
87 \n\
88This demonstration explains also how to move the object around a world\n\
89reference frame. Here, the movement is a rotation around the x and y axis\n\
90at a given distance from the world frame. In fact the object trajectory\n\
91is on a sphere whose center is the origin of the world frame.\n\
92 \n\
93SYNOPSIS\n\
94 %s [-d] [-p] [-h]\n",
95 name);
96
97 fprintf(stdout, "\n\
98OPTIONS: Default\n\
99 -d \n\
100 Turn off the display.\n\
101 \n\
102 -p \n\
103 Turn off the plotter.\n\
104 \n\
105 -h\n\
106 Print the help.\n");
107
108 if (badparam)
109 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
110}
111
124bool getOptions(int argc, const char **argv, bool &display, bool &plot)
125{
126 const char *optarg_;
127 int c;
128 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
129
130 switch (c) {
131 case 'd':
132 display = false;
133 break;
134 case 'p':
135 plot = false;
136 break;
137 case 'h':
138 usage(argv[0], NULL);
139 return false;
140
141 default:
142 usage(argv[0], optarg_);
143 return false;
144 }
145 }
146
147 if ((c == 1) || (c == -1)) {
148 // standalone param or error
149 usage(argv[0], NULL);
150 std::cerr << "ERROR: " << std::endl;
151 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
152 return false;
153 }
154
155 return true;
156}
157
158int main(int argc, const char **argv)
159{
160 try {
161 bool opt_display = true;
162 bool opt_plot = true;
163 std::string filename = vpIoTools::getParent(argv[0]) + "/mire.png";
164 std::cout << "Read " << filename << std::endl;
165
166 // Read the command line options
167 if (getOptions(argc, argv, opt_display, opt_plot) == false) {
168 return EXIT_FAILURE;
169 }
170
171 vpImage<vpRGBa> Iint(480, 640, 255);
172 vpImage<vpRGBa> Iext1(480, 640, 255);
173 vpImage<vpRGBa> Iext2(480, 640, 255);
174
175#if defined(VISP_HAVE_X11)
176 vpDisplayX display[3];
177#elif defined(HAVE_OPENCV_HIGHGUI)
178 vpDisplayOpenCV display[3];
179#elif defined(VISP_HAVE_GDI)
180 vpDisplayGDI display[3];
181#elif defined(VISP_HAVE_D3D9)
182 vpDisplayD3D display[3];
183#elif defined(VISP_HAVE_GTK)
184 vpDisplayGTK display[3];
185#endif
186
187 if (opt_display) {
188 // Display size is automatically defined by the image (I) size
189 display[0].init(Iint, 100, 100, "The internal view");
190 display[1].init(Iext1, 100, 100, "The first external view");
191 display[2].init(Iext2, 100, 100, "The second external view");
193 vpDisplay::setWindowPosition(Iext1, 750, 0);
194 vpDisplay::setWindowPosition(Iext2, 0, 550);
195 vpDisplay::display(Iint);
196 vpDisplay::flush(Iint);
197 vpDisplay::display(Iext1);
198 vpDisplay::flush(Iext1);
199 vpDisplay::display(Iext2);
200 vpDisplay::flush(Iext2);
201 }
202
203 vpPlot *plotter = NULL;
204
205 if (opt_plot) {
206 plotter = new vpPlot(2, 480, 640, 750, 550, "Real time curves plotter");
207 plotter->setTitle(0, "Visual features error");
208 plotter->setTitle(1, "Camera velocities");
209 plotter->initGraph(0, 8);
210 plotter->initGraph(1, 6);
211 plotter->setLegend(0, 0, "error_feat_p1_x");
212 plotter->setLegend(0, 1, "error_feat_p1_y");
213 plotter->setLegend(0, 2, "error_feat_p2_x");
214 plotter->setLegend(0, 3, "error_feat_p2_y");
215 plotter->setLegend(0, 4, "error_feat_p3_x");
216 plotter->setLegend(0, 5, "error_feat_p3_y");
217 plotter->setLegend(0, 6, "error_feat_p4_x");
218 plotter->setLegend(0, 7, "error_feat_p4_y");
219 plotter->setLegend(1, 0, "vc_x");
220 plotter->setLegend(1, 1, "vc_y");
221 plotter->setLegend(1, 2, "vc_z");
222 plotter->setLegend(1, 3, "wc_x");
223 plotter->setLegend(1, 4, "wc_y");
224 plotter->setLegend(1, 5, "wc_z");
225 }
226
227 vpServo task;
228 vpSimulatorCamera robot;
229 float sampling_time = 0.020f; // Sampling period in second
230 robot.setSamplingTime(sampling_time);
231
232 // Since the task gain lambda is very high, we need to increase default
233 // max velocities
236
237 // Set initial position of the object in the camera frame
238 vpHomogeneousMatrix cMo(0, 0.1, 2.0, vpMath::rad(35), vpMath::rad(25), 0);
239 // Set desired position of the object in the camera frame
240 vpHomogeneousMatrix cdMo(0.0, 0.0, 1.0, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
241 // Set initial position of the object in the world frame
242 vpHomogeneousMatrix wMo(0.0, 0.0, 0.2, 0, 0, 0);
243 // Position of the camera in the world frame
245 wMc = wMo * cMo.inverse();
246
247 // The four point used as visual features
248 vpPoint point[4];
249 point[0].setWorldCoordinates(-0.1, -0.1, 0);
250 point[3].setWorldCoordinates(-0.1, 0.1, 0);
251 point[2].setWorldCoordinates(0.1, 0.1, 0);
252 point[1].setWorldCoordinates(0.1, -0.1, 0);
253
254 // Projection of the points
255 for (int i = 0; i < 4; i++)
256 point[i].track(cMo);
257
258 // Set the current visual feature
259 vpFeaturePoint p[4];
260 for (int i = 0; i < 4; i++)
261 vpFeatureBuilder::create(p[i], point[i]);
262
263 // Projection of the points
264 for (int i = 0; i < 4; i++)
265 point[i].track(cdMo);
266
267 vpFeaturePoint pd[4];
268 for (int i = 0; i < 4; i++)
269 vpFeatureBuilder::create(pd[i], point[i]);
270
273
274 vpHomogeneousMatrix cMe; // Identity
275 vpVelocityTwistMatrix cVe(cMe);
276 task.set_cVe(cVe);
277
278 vpMatrix eJe;
279 robot.get_eJe(eJe);
280 task.set_eJe(eJe);
281
282 for (int i = 0; i < 4; i++)
283 task.addFeature(p[i], pd[i]);
284
285 task.setLambda(10);
286
287 std::list<vpImageSimulator> list;
288 vpImageSimulator imsim;
289
290 vpColVector X[4];
291 for (int i = 0; i < 4; i++)
292 X[i].resize(3);
293 X[0][0] = -0.2;
294 X[0][1] = -0.2;
295 X[0][2] = 0;
296
297 X[1][0] = 0.2;
298 X[1][1] = -0.2;
299 X[1][2] = 0;
300
301 X[2][0] = 0.2;
302 X[2][1] = 0.2;
303 X[2][2] = 0;
304
305 X[3][0] = -0.2;
306 X[3][1] = 0.2;
307 X[3][2] = 0;
308
309 imsim.init(filename.c_str(), X);
310
311 list.push_back(imsim);
312
314
315 // Set the scene
317
318 // Initialize simulator frames
319 sim.set_fMo(wMo); // Position of the object in the world reference frame
320 sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
321 sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
322
323 // Set the External camera position
324 vpHomogeneousMatrix camMf(vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0));
325 sim.setExternalCameraPosition(camMf);
326
327 // Computes the position of a camera which is fixed in the object frame
328 vpHomogeneousMatrix camoMf(0, 0.0, 1.5, 0, vpMath::rad(140), 0);
329 camoMf = camoMf * (sim.get_fMo().inverse());
330
331 // Set the parameters of the cameras (internal and external)
332 vpCameraParameters camera(1000, 1000, 320, 240);
333 sim.setInternalCameraParameters(camera);
334 sim.setExternalCameraParameters(camera);
335
336 int max_iter = 10;
337
338 if (opt_display) {
339 max_iter = 2500;
340
341 // Get the internal and external views
342 sim.getInternalImage(Iint);
343 sim.getExternalImage(Iext1);
344 sim.getExternalImage(Iext2, camoMf);
345
346 // Display the object frame (current and desired position)
347 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
348 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
349
350 // Display the object frame the world reference frame and the camera
351 // frame
352 vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
353 vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
354 vpDisplay::displayFrame(Iext1, camMf, camera, 0.2, vpColor::none);
355
356 // Display the world reference frame and the object frame
357 vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
358 vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
359
360 vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo", vpColor::red);
361
362 vpDisplay::flush(Iint);
363 vpDisplay::flush(Iext1);
364 vpDisplay::flush(Iext2);
365
366 std::cout << "Click on a display" << std::endl;
367
368 while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) &&
369 !vpDisplay::getClick(Iext2, false)) {
370 };
371 }
372
373 robot.setPosition(wMc);
374 // Print the task
375 task.print();
376
377 int iter = 0;
378 bool stop = false;
379 vpColVector v;
380
381 double t_prev, t = vpTime::measureTimeMs();
382
383 while (iter++ < max_iter && !stop) {
384 t_prev = t;
386
387 if (opt_display) {
388 vpDisplay::display(Iint);
389 vpDisplay::display(Iext1);
390 vpDisplay::display(Iext2);
391 }
392
393 robot.get_eJe(eJe);
394 task.set_eJe(eJe);
395
396 wMc = robot.getPosition();
397 cMo = wMc.inverse() * wMo;
398 for (int i = 0; i < 4; i++) {
399 point[i].track(cMo);
400 vpFeatureBuilder::create(p[i], point[i]);
401 }
402
403 v = task.computeControlLaw();
405
406 // Compute the movement of the object around the world reference frame.
407 vpHomogeneousMatrix a(0, 0, 0.2, 0, 0, 0);
408 vpHomogeneousMatrix b(0, 0, 0, vpMath::rad(1.5 * iter), 0, 0);
409 vpHomogeneousMatrix c(0, 0, 0, 0, vpMath::rad(2.5 * iter), 0);
410
411 // Move the object in the world frame
412 wMo = b * c * a;
413
414 sim.set_fMo(wMo); // Move the object in the simulator
416
417 // Compute the position of the external view which is fixed in the
418 // object frame
419 camoMf.buildFrom(0, 0.0, 1.5, 0, vpMath::rad(150), 0);
420 camoMf = camoMf * (sim.get_fMo().inverse());
421
422 if (opt_plot) {
423 plotter->plot(0, iter, task.getError());
424 plotter->plot(1, iter, v);
425 }
426
427 if (opt_display) {
428 // Get the internal and external views
429 sim.getInternalImage(Iint);
430 sim.getExternalImage(Iext1);
431 sim.getExternalImage(Iext2, camoMf);
432
433 // Display the object frame (current and desired position)
434 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
435 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
436
437 // Display the camera frame, the object frame the world reference
438 // frame
439 vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
443
444 // Display the world reference frame and the object frame
445 vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
446 vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
447
448 vpDisplay::displayText(Iint, 20, 20, "Click to stop visual servo", vpColor::red);
449
450 std::stringstream ss;
451 ss << "Loop time: " << t - t_prev << " ms";
452 vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::red);
453
454 if (vpDisplay::getClick(Iint, false)) {
455 stop = true;
456 }
457 vpDisplay::flush(Iint);
458 vpDisplay::flush(Iext1);
459 vpDisplay::flush(Iext2);
460
461 vpTime::wait(t, sampling_time * 1000); // Wait ms
462 }
463
464 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
465 }
466
467 if (opt_plot && plotter != NULL) {
468 vpDisplay::display(Iint);
469 sim.getInternalImage(Iint);
470 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
471 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
472 vpDisplay::displayText(Iint, 20, 20, "Click to quit", vpColor::red);
473 if (vpDisplay::getClick(Iint)) {
474 stop = true;
475 }
476 vpDisplay::flush(Iint);
477
478 delete plotter;
479 }
480
481 task.print();
482
483 return EXIT_SUCCESS;
484 }
485 catch (const vpException &e) {
486 std::cout << "Catch an exception: " << e << std::endl;
487 return EXIT_FAILURE;
488 }
489}
490#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
491int main()
492{
493 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
494 return EXIT_SUCCESS;
495}
496#else
497int main()
498{
499 std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..."
500 << std::endl;
501 std::cout << "Tip if you are on a unix-like system:" << std::endl;
502 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
503 std::cout << "Tip if you are on a windows-like system:" << std::endl;
504 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
505 return EXIT_SUCCESS;
506}
507
508#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
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void init(const vpImage< unsigned char > &I, vpColVector *X)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static std::string getParent(const std::string &pathname)
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
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
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
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.
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)
void getExternalImage(vpImage< unsigned char > &I)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()