Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuFourPoints2DPolarCamVelocityDisplay.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 using 4 points with polar
33 * coordinates as visual feature.
34 *
35*****************************************************************************/
36
53#include <visp3/core/vpConfig.h>
54#include <visp3/core/vpDebug.h>
55
56#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
57
58#include <stdio.h>
59#include <stdlib.h>
60
61#include <visp3/core/vpCameraParameters.h>
62#include <visp3/core/vpHomogeneousMatrix.h>
63#include <visp3/core/vpImage.h>
64#include <visp3/core/vpImagePoint.h>
65#include <visp3/core/vpIoTools.h>
66#include <visp3/core/vpMath.h>
67#include <visp3/core/vpMeterPixelConversion.h>
68#include <visp3/gui/vpDisplayGDI.h>
69#include <visp3/gui/vpDisplayGTK.h>
70#include <visp3/gui/vpDisplayOpenCV.h>
71#include <visp3/gui/vpDisplayX.h>
72#include <visp3/gui/vpProjectionDisplay.h>
73#include <visp3/io/vpParseArgv.h>
74#include <visp3/robot/vpSimulatorCamera.h>
75#include <visp3/visual_features/vpFeatureBuilder.h>
76#include <visp3/visual_features/vpFeaturePointPolar.h>
77#include <visp3/vs/vpServo.h>
78#include <visp3/vs/vpServoDisplay.h>
79
80// List of allowed command line options
81#define GETOPTARGS "cdh"
82
83void usage(const char *name, const char *badparam);
84bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
85
94void usage(const char *name, const char *badparam)
95{
96 fprintf(stdout, "\n\
97Tests a control law with the following characteristics:\n\
98- eye-in-hand control\n\
99- articular velocity are computed\n\
100- servo on 4 points,\n\
101- internal and external camera view displays.\n\
102\n\
103SYNOPSIS\n\
104 %s [-c] [-d] [-h]\n",
105 name);
106
107 fprintf(stdout, "\n\
108OPTIONS: Default\n\
109 -c\n\
110 Disable the mouse click. Useful to automate the \n\
111 execution of this program without human intervention.\n\
112\n\
113 -d \n\
114 Turn off the display.\n\
115\n\
116 -h\n\
117 Print the help.\n");
118
119 if (badparam)
120 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
121}
134bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
135{
136 const char *optarg_;
137 int c;
138 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
139
140 switch (c) {
141 case 'c':
142 click_allowed = false;
143 break;
144 case 'd':
145 display = false;
146 break;
147 case 'h':
148 usage(argv[0], NULL);
149 return false;
150
151 default:
152 usage(argv[0], optarg_);
153 return false;
154 }
155 }
156
157 if ((c == 1) || (c == -1)) {
158 // standalone param or error
159 usage(argv[0], NULL);
160 std::cerr << "ERROR: " << std::endl;
161 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
162 return false;
163 }
164
165 return true;
166}
167
168int main(int argc, const char **argv)
169{
170 try {
171 // Log file creation in /tmp/$USERNAME/log.dat
172 // This file contains by line:
173 // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
174 // - the 6 mesured camera velocities (m/s, rad/s)
175 // - the 6 mesured joint positions (m, rad)
176 // - the 8 values of s - s*
177 std::string username;
178 // Get the user login name
179 vpIoTools::getUserName(username);
180
181 // Create a log filename to save velocities...
182 std::string logdirname;
183#if defined(_WIN32)
184 logdirname = "C:/temp/" + username;
185#else
186 logdirname = "/tmp/" + username;
187#endif
188
189 // Test if the output path exist. If no try to create it
190 if (vpIoTools::checkDirectory(logdirname) == false) {
191 try {
192 // Create the dirname
193 vpIoTools::makeDirectory(logdirname);
194 } catch (...) {
195 std::cerr << std::endl << "ERROR:" << std::endl;
196 std::cerr << " Cannot create " << logdirname << std::endl;
197 return EXIT_FAILURE;
198 }
199 }
200 std::string logfilename;
201 logfilename = logdirname + "/log.dat";
202
203 // Open the log file name
204 std::ofstream flog(logfilename.c_str());
205
206 bool opt_click_allowed = true;
207 bool opt_display = true;
208
209 // Read the command line options
210 if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
211 return EXIT_FAILURE;
212 }
213
214// We open two displays, one for the internal camera view, the other one for
215// the external view, using either X11, GTK or GDI.
216#if defined(VISP_HAVE_X11)
217 vpDisplayX displayInt;
218 vpDisplayX displayExt;
219#elif defined(VISP_HAVE_GTK)
220 vpDisplayGTK displayInt;
221 vpDisplayGTK displayExt;
222#elif defined(VISP_HAVE_GDI)
223 vpDisplayGDI displayInt;
224 vpDisplayGDI displayExt;
225#elif defined(HAVE_OPENCV_HIGHGUI)
226 vpDisplayOpenCV displayInt;
227 vpDisplayOpenCV displayExt;
228#endif
229
230 // open a display for the visualization
231
232 vpImage<unsigned char> Iint(300, 300, 0);
233 vpImage<unsigned char> Iext(300, 300, 0);
234
235 if (opt_display) {
236 displayInt.init(Iint, 0, 0, "Internal view");
237 displayExt.init(Iext, 330, 000, "External view");
238 }
239 vpProjectionDisplay externalview;
240
241 double px = 500, py = 500;
242 double u0 = 150, v0 = 160;
243
244 vpCameraParameters cam(px, py, u0, v0);
245
246 vpServo task;
247 vpSimulatorCamera robot;
248
249 std::cout << std::endl;
250 std::cout << "----------------------------------------------" << std::endl;
251 std::cout << " Test program for vpServo " << std::endl;
252 std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
253 std::cout << " Simulation " << std::endl;
254 std::cout << " task : servo 4 points " << std::endl;
255 std::cout << "----------------------------------------------" << std::endl;
256 std::cout << std::endl;
257
258// #define TRANS_Z_PURE
259// #define TRANS_X_PURE
260// #define ROT_Z_PURE
261// #define ROT_X_PURE
262#define COMPLEX
263 //#define PROBLEM
264
265#if defined(TRANS_Z_PURE)
266 // sets the initial camera location
268 // sets the desired camera location
269 vpHomogeneousMatrix cMod(0, 0, 2, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
270#elif defined(TRANS_X_PURE)
271 // sets the initial camera location
272 vpHomogeneousMatrix cMo(0.3, 0.3, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
273 // sets the desired camera location
274 vpHomogeneousMatrix cMod(0.5, 0.3, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
275
276#elif defined(ROT_Z_PURE)
277 // sets the initial camera location
279 // sets the desired camera location
280 vpHomogeneousMatrix cMod(0, 0, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(180));
281
282#elif defined(ROT_X_PURE)
283 // sets the initial camera location
285 // sets the desired camera location
286 vpHomogeneousMatrix cMod(0, 0, 3, vpMath::rad(45), vpMath::rad(0), vpMath::rad(0));
287
288#elif defined(COMPLEX)
289 // sets the initial camera location
290 vpHomogeneousMatrix cMo(0.2, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
291 // sets the desired camera location
292 vpHomogeneousMatrix cMod(0, 0, 2.5, vpMath::rad(45), vpMath::rad(10), vpMath::rad(30));
293
294#elif defined(PROBLEM)
295 // Bad behavior with an interaction matrix computed from the desired
296 // features sets the initial camera location
297 vpHomogeneousMatrix cMo(0.2, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
298 // sets the desired camera location
299 vpHomogeneousMatrix cMod(0.4, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
300
301#endif
302 // Compute the position of the object in the world frame
303 vpHomogeneousMatrix wMc, wMo;
304 robot.getPosition(wMc);
305 wMo = wMc * cMo;
306
307 vpHomogeneousMatrix cextMo(0, 0, 6, vpMath::rad(40), vpMath::rad(10), vpMath::rad(60));
308
309 // sets the point coordinates in the object frame
310 vpPoint point[4];
311 point[0].setWorldCoordinates(-0.25, -0.25, 0);
312 point[1].setWorldCoordinates(0.25, -0.25, 0);
313 point[2].setWorldCoordinates(0.25, 0.25, 0);
314 point[3].setWorldCoordinates(-0.25, 0.25, 0);
315
316 for (unsigned int i = 0; i < 4; i++)
317 externalview.insert(point[i]);
318
319 // sets the desired position of the feature point s*"
321
322 // computes the point coordinates in the desired camera frame and
323 // its 2D coordinates
324 for (unsigned int i = 0; i < 4; i++) {
325 point[i].track(cMod);
326 // Computes the polar coordinates from the image point
327 // cartesian coordinates
328 vpFeatureBuilder::create(pd[i], point[i]);
329 }
330
331 // computes the point coordinates in the camera frame and its 2D
332 // coordinates
333 for (unsigned int i = 0; i < 4; i++)
334 point[i].track(cMo);
335
336 // sets the desired position of the point
338 for (unsigned int i = 0; i < 4; i++) {
339 // retrieve x,y and Z of the vpPoint structure to initialize the
340 // visual feature
341 vpFeatureBuilder::create(p[i], point[i]);
342 }
343
344 // Define the task;
345 // - we want an eye-in-hand control law
346 // - articular velocity are computed
348 // task.setInteractionMatrixType(vpServo::MEAN) ;
349 // task.setInteractionMatrixType(vpServo::DESIRED) ;
351
352 // Set the position of the end-effector frame in the camera frame as identity
354 vpVelocityTwistMatrix cVe(cMe);
355 task.set_cVe(cVe);
356
357 // Set the Jacobian (expressed in the end-effector frame)
358 vpMatrix eJe;
359 robot.get_eJe(eJe);
360 task.set_eJe(eJe);
361
362 // we want to see a point on a point
363 for (unsigned int i = 0; i < 4; i++)
364 task.addFeature(p[i], pd[i]);
365
366 // set the gain
367 task.setLambda(1);
368
369 std::cout << "\nDisplay task information: " << std::endl;
370 task.print();
371
372 unsigned int iter = 0;
373 // loop
374 while (iter++ < 200) {
375 std::cout << "---------------------------------------------" << iter << std::endl;
376 vpColVector v;
377
378 // Set the Jacobian (expressed in the end-effector frame)
379 // Since q is modified eJe is modified
380 robot.get_eJe(eJe);
381 task.set_eJe(eJe);
382
383 // get the robot position
384 robot.getPosition(wMc);
385 // Compute the position of the object frame in the camera frame
386 cMo = wMc.inverse() * wMo;
387
388 // Compute new point position
389 for (unsigned int i = 0; i < 4; i++) {
390 point[i].track(cMo);
391 // retrieve x,y and Z of the vpPoint structure to compute the feature
392 vpFeatureBuilder::create(p[i], point[i]);
393 }
394
395 if (opt_display) {
396 vpDisplay::display(Iint);
397 vpDisplay::display(Iext);
398
399 vpServoDisplay::display(task, cam, Iint);
400 externalview.display(Iext, cextMo, cMo, cam, vpColor::green);
401 vpDisplay::flush(Iint);
402 vpDisplay::flush(Iext);
403 }
404
405 // Compute the control law
406 v = task.computeControlLaw();
407
408 if (iter == 1) {
409 std::cout << "Display task information: " << std::endl;
410 task.print();
411 }
412
415
416 // Send the camera velocity to the controller
418 // Save velocities applied to the robot in the log file
419 // v[0], v[1], v[2] correspond to camera translation velocities in m/s
420 // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
421 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
422
423 std::cout << "v: " << v.t() << std::endl;
424
425 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
426
427 // Save feature error (s-s*) for the 4 feature points. For each feature
428 // point, we have 2 errors (along x and y axis). This error is
429 // expressed in meters in the camera frame
430 flog << (task.getError()).t() << " "; // s-s* for point 4
431 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
432
433 // Save current visual feature s = (rho,theta)
434 for (unsigned int i = 0; i < 4; i++) {
435 flog << p[i].get_rho() << " " << p[i].get_theta() << " ";
436 }
437 // Save current position of the points
438 for (unsigned int i = 0; i < 4; i++) {
439 flog << point[i].get_x() << " " << point[i].get_y() << " ";
440 }
441 flog << std::endl;
442
443 if (iter == 1) {
444 vpImagePoint ip;
445 ip.set_i(10);
446 ip.set_j(10);
447
448 std::cout << "\nClick in the internal camera view to continue..." << std::endl;
449 vpDisplay::displayText(Iint, ip, "A click to continue...", vpColor::red);
450 vpDisplay::flush(Iint);
452 }
453 }
454
455 flog.close(); // Close the log file
456
457 // Display task information
458 task.print();
459
460 // Kill the task
461
462 std::cout << "Final robot position with respect to the object frame:\n";
463 cMo.print();
464
465 if (opt_display && opt_click_allowed) {
466 vpDisplay::displayText(Iint, 20, 20, "Click to quit...", vpColor::white);
467 vpDisplay::flush(Iint);
469 }
470 return EXIT_SUCCESS;
471 } catch (const vpException &e) {
472 std::cout << "Catch a ViSP exception: " << e << std::endl;
473 return EXIT_FAILURE;
474 }
475}
476#else
477int main()
478{
479 std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..."
480 << std::endl;
481 std::cout << "Tip if you are on a unix-like system:" << std::endl;
482 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
483 std::cout << "Tip if you are on a windows-like system:" << std::endl;
484 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
485 return EXIT_SUCCESS;
486}
487#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
vpRowVector t() const
static const vpColor white
Definition vpColor.h:206
static const vpColor red
Definition vpColor.h:211
static const vpColor green
Definition vpColor.h:214
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
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
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 2D image point visual feature with polar coordinates described in .
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_j(double jj)
void set_i(double ii)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static bool checkDirectory(const std::string &dirname)
static std::string getUserName()
static void makeDirectory(const std::string &dirname)
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)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
interface with the image for feature display
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
void insert(vpForwardProjection &fp)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ 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_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
@ FEATURE_CURRENT
Definition vpServo.h:208
@ FEATURE_DESIRED
Definition vpServo.h:209
@ CURRENT
Definition vpServo.h:179
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.