Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuSquareLine2DCamVelocityDisplay.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 line.
33 *
34*****************************************************************************/
35
45#include <visp3/core/vpConfig.h>
46#include <visp3/core/vpDebug.h>
47
48#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
49 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
50
51#include <stdio.h>
52#include <stdlib.h>
53
54#include <visp3/core/vpCameraParameters.h>
55#include <visp3/core/vpHomogeneousMatrix.h>
56#include <visp3/core/vpImage.h>
57#include <visp3/core/vpLine.h>
58#include <visp3/core/vpMath.h>
59#include <visp3/gui/vpDisplayGDI.h>
60#include <visp3/gui/vpDisplayGTK.h>
61#include <visp3/gui/vpDisplayOpenCV.h>
62#include <visp3/gui/vpDisplayX.h>
63#include <visp3/io/vpParseArgv.h>
64#include <visp3/robot/vpSimulatorCamera.h>
65#include <visp3/visual_features/vpFeatureBuilder.h>
66#include <visp3/visual_features/vpFeatureLine.h>
67#include <visp3/vs/vpServo.h>
68#include <visp3/vs/vpServoDisplay.h>
69
70// List of allowed command line options
71#define GETOPTARGS "cdh"
72
73void usage(const char *name, const char *badparam);
74bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
75
84void usage(const char *name, const char *badparam)
85{
86 fprintf(stdout, "\n\
87Simulation of 2D a visual servoing on a line:\n\
88- eye-in-hand control law,\n\
89- velocity computed in the camera frame,\n\
90- display the camera view.\n\
91 \n\
92SYNOPSIS\n\
93 %s [-c] [-d] [-h]\n",
94 name);
95
96 fprintf(stdout, "\n\
97OPTIONS: Default\n\
98 \n\
99 -c\n\
100 Disable the mouse click. Useful to automate the \n\
101 execution of this program without human intervention.\n\
102 \n\
103 -d \n\
104 Turn off the display.\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 &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, 0);
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 } catch (...) {
195 vpERROR_TRACE("Error while displaying the image");
196 return EXIT_FAILURE;
197 }
198 }
199
200 // Set the camera parameters
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.2, 1, vpMath::rad(45), vpMath::rad(45), vpMath::rad(125));
213
214 // Compute the position of the object in the world frame
215 vpHomogeneousMatrix wMc, wMo;
216 robot.getPosition(wMc);
217 wMo = wMc * cMo;
218
219 // sets the final camera location (for simulation purpose)
220 vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
221
222 int nbline = 4;
223
224 // sets the line coordinates (2 planes) in the world frame
225 vpLine line[4];
226 line[0].setWorldCoordinates(1, 0, 0, 0.05, 0, 0, 1, 0);
227 line[1].setWorldCoordinates(0, 1, 0, 0.05, 0, 0, 1, 0);
228 line[2].setWorldCoordinates(1, 0, 0, -0.05, 0, 0, 1, 0);
229 line[3].setWorldCoordinates(0, 1, 0, -0.05, 0, 0, 1, 0);
230
231 vpFeatureLine ld[4];
232 vpFeatureLine l[4];
233
234 // sets the desired position of the visual feature
235 for (int i = 0; i < nbline; i++) {
236 line[i].track(cMod);
237 line[i].print();
238
239 vpFeatureBuilder::create(ld[i], line[i]);
240 }
241
242 // computes the line coordinates in the camera frame and its 2D
243 // coordinates sets the current position of the visual feature
244 for (int i = 0; i < nbline; i++) {
245 line[i].track(cMo);
246 line[i].print();
247
248 vpFeatureBuilder::create(l[i], line[i]);
249 l[i].print();
250 }
251
252 // define the task
253 // - we want an eye-in-hand control law
254 // - robot is controlled in the camera frame
257 // It could be also interesting to test the following tasks
258 // task.setInteractionMatrixType(vpServo::DESIRED,
259 // vpServo::PSEUDO_INVERSE); task.setInteractionMatrixType(vpServo::MEAN,
260 // vpServo::PSEUDO_INVERSE);
261
262 // we want to see a four lines on four lines
263 for (int i = 0; i < nbline; i++)
264 task.addFeature(l[i], ld[i]);
265
267 vpServoDisplay::display(task, cam, I);
269
270 // set the gain
271 task.setLambda(1);
272
273 // Display task information
274 task.print();
275
276 if (opt_display && opt_click_allowed) {
277 std::cout << "\n\nClick in the camera view window to start..." << std::endl;
279 }
280
281 unsigned int iter = 0;
282 // loop
283 while (iter++ < 200) {
284 std::cout << "---------------------------------------------" << iter << std::endl;
285 vpColVector v;
286
287 // get the robot position
288 robot.getPosition(wMc);
289 // Compute the position of the object frame in the camera frame
290 cMo = wMc.inverse() * wMo;
291
292 // new line position: retrieve x,y and Z of the vpLine structure
293 for (int i = 0; i < nbline; i++) {
294 line[i].track(cMo);
295 vpFeatureBuilder::create(l[i], line[i]);
296 }
297
298 if (opt_display) {
300 vpServoDisplay::display(task, cam, I);
302 }
303
304 // compute the control law
305 v = task.computeControlLaw();
306
307 // send the camera velocity to the controller
309
310 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
311 }
312
313 if (opt_display && opt_click_allowed) {
314 vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::white);
317 }
318
319 // Display task information
320 task.print();
321 return EXIT_SUCCESS;
322 } catch (const vpException &e) {
323 std::cout << "Catch a ViSP exception: " << e << std::endl;
324 return EXIT_FAILURE;
325 }
326}
327
328#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
329int main()
330{
331 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
332 return EXIT_SUCCESS;
333}
334#else
335int main()
336{
337 std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) or OpenCV functionalities to display "
338 "images..."
339 << std::endl;
340 std::cout << "Tip if you are on a unix-like system:" << std::endl;
341 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
342 std::cout << "Tip if you are on a windows-like system:" << std::endl;
343 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
344 return EXIT_SUCCESS;
345}
346#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor white
Definition vpColor.h:206
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
virtual void print() const
void track(const vpHomogeneousMatrix &cMo)
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
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition vpLine.h:100
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)
Definition vpLine.cpp:82
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
@ 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.
#define vpERROR_TRACE
Definition vpDebug.h:388