Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuFourPoints2DCamVelocityDisplay.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 as visual feature.
33 *
34*****************************************************************************/
35
52#include <iostream>
53
54#include <visp3/core/vpConfig.h>
55
56#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
57 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
58
59#include <stdio.h>
60#include <stdlib.h>
61
62#include <visp3/core/vpCameraParameters.h>
63#include <visp3/core/vpHomogeneousMatrix.h>
64#include <visp3/core/vpImage.h>
65#include <visp3/core/vpMath.h>
66#include <visp3/gui/vpDisplayGDI.h>
67#include <visp3/gui/vpDisplayGTK.h>
68#include <visp3/gui/vpDisplayOpenCV.h>
69#include <visp3/gui/vpDisplayX.h>
70#include <visp3/gui/vpProjectionDisplay.h>
71#include <visp3/io/vpParseArgv.h>
72#include <visp3/robot/vpSimulatorCamera.h>
73#include <visp3/visual_features/vpFeatureBuilder.h>
74#include <visp3/visual_features/vpFeaturePoint.h>
75#include <visp3/vs/vpServo.h>
76#include <visp3/vs/vpServoDisplay.h>
77
78// List of allowed command line options
79#define GETOPTARGS "cdh"
80
81void usage(const char *name, const char *badparam);
82bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
83
92void usage(const char *name, const char *badparam)
93{
94 fprintf(stdout, "\n\
95Tests a control law with the following characteristics:\n\
96- eye-in-hand control\n\
97- articular velocity are computed\n\
98- servo on 4 points,\n\
99- internal and external camera view displays.\n\
100 \n\
101SYNOPSIS\n\
102 %s [-c] [-d] [-h]\n",
103 name);
104
105 fprintf(stdout, "\n\
106OPTIONS: Default\n\
107 -c\n\
108 Disable the mouse click. Useful to automate the \n\
109 execution of this program without human intervention.\n\
110 \n\
111 -d \n\
112 Turn off the display.\n\
113 \n\
114 -h\n\
115 Print the help.\n");
116
117 if (badparam)
118 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
119}
132bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
133{
134 const char *optarg_;
135 int c;
136 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
137
138 switch (c) {
139 case 'c':
140 click_allowed = false;
141 break;
142 case 'd':
143 display = false;
144 break;
145 case 'h':
146 usage(argv[0], NULL);
147 return false;
148
149 default:
150 usage(argv[0], optarg_);
151 return false;
152 }
153 }
154
155 if ((c == 1) || (c == -1)) {
156 // standalone param or error
157 usage(argv[0], NULL);
158 std::cerr << "ERROR: " << std::endl;
159 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
160 return false;
161 }
162
163 return true;
164}
165
166int main(int argc, const char **argv)
167{
168 try {
169 bool opt_click_allowed = true;
170 bool opt_display = true;
171
172 // Read the command line options
173 if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
174 return EXIT_FAILURE;
175 }
176
177// We open two displays, one for the internal camera view, the other one for
178// the external view, using either X11, GTK or GDI.
179#if defined(VISP_HAVE_X11)
180 vpDisplayX displayInt;
181 vpDisplayX displayExt;
182#elif defined(VISP_HAVE_GTK)
183 vpDisplayGTK displayInt;
184 vpDisplayGTK displayExt;
185#elif defined(VISP_HAVE_GDI)
186 vpDisplayGDI displayInt;
187 vpDisplayGDI displayExt;
188#elif defined(HAVE_OPENCV_HIGHGUI)
189 vpDisplayOpenCV displayInt;
190 vpDisplayOpenCV displayExt;
191#endif
192
193 // open a display for the visualization
194
195 vpImage<unsigned char> Iint(300, 300, 0);
196 vpImage<unsigned char> Iext(300, 300, 0);
197
198 if (opt_display) {
199 displayInt.init(Iint, 0, 0, "Internal view");
200 displayExt.init(Iext, 330, 000, "External view");
201 }
202 vpProjectionDisplay externalview;
203
204 double px = 500, py = 500;
205 double u0 = 150, v0 = 160;
206
207 vpCameraParameters cam(px, py, u0, v0);
208
209 vpServo task;
210 vpSimulatorCamera robot;
211
212 std::cout << std::endl;
213 std::cout << "----------------------------------------------" << std::endl;
214 std::cout << " Test program for vpServo " << std::endl;
215 std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
216 std::cout << " Simulation " << std::endl;
217 std::cout << " task : servo 4 points " << std::endl;
218 std::cout << "----------------------------------------------" << std::endl;
219 std::cout << std::endl;
220
221 // sets the initial camera location
222 vpHomogeneousMatrix cMo(-0.1, -0.1, 1, vpMath::rad(40), vpMath::rad(10), vpMath::rad(60));
223
224 // Compute the position of the object in the world frame
225 vpHomogeneousMatrix wMc, wMo;
226 robot.getPosition(wMc);
227 wMo = wMc * cMo;
228
229 vpHomogeneousMatrix cextMo(0, 0, 2, 0, 0, 0); // vpMath::rad(40), vpMath::rad(10), vpMath::rad(60));
230
231 // sets the point coordinates in the object frame
232 vpPoint point[4];
233 point[0].setWorldCoordinates(-0.1, -0.1, 0);
234 point[1].setWorldCoordinates(0.1, -0.1, 0);
235 point[2].setWorldCoordinates(0.1, 0.1, 0);
236 point[3].setWorldCoordinates(-0.1, 0.1, 0);
237
238 for (unsigned i = 0; i < 4; i++)
239 externalview.insert(point[i]);
240
241 // computes the point coordinates in the camera frame and its 2D
242 // coordinates
243 for (unsigned i = 0; i < 4; i++)
244 point[i].track(cMo);
245
246 // sets the desired position of the point
247 vpFeaturePoint p[4];
248 for (unsigned i = 0; i < 4; i++)
249 vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
250
251 // sets the desired position of the feature point s*
252 vpFeaturePoint pd[4];
253
254 pd[0].buildFrom(-0.1, -0.1, 1);
255 pd[1].buildFrom(0.1, -0.1, 1);
256 pd[2].buildFrom(0.1, 0.1, 1);
257 pd[3].buildFrom(-0.1, 0.1, 1);
258
259 // define the task
260 // - we want an eye-in-hand control law
261 // - articular velocity are computed
264
265 // Set the position of the end-effector frame in the camera frame as identity
267 vpVelocityTwistMatrix cVe(cMe);
268 task.set_cVe(cVe);
269
270 // Set the Jacobian (expressed in the end-effector frame
271 vpMatrix eJe;
272 robot.get_eJe(eJe);
273 task.set_eJe(eJe);
274
275 // we want to see a point on a point
276 for (unsigned i = 0; i < 4; i++)
277 task.addFeature(p[i], pd[i]);
278
279 // set the gain
280 task.setLambda(1);
281
282 // Display task information
283 task.print();
284
285 unsigned int iter = 0;
286 // loop
287 while (iter++ < 200) {
288 std::cout << "---------------------------------------------" << iter << std::endl;
289 vpColVector v;
290
291 // Set the Jacobian (expressed in the end-effector frame)
292 // since q is modified eJe is modified
293 robot.get_eJe(eJe);
294 task.set_eJe(eJe);
295
296 // get the robot position
297 robot.getPosition(wMc);
298 // Compute the position of the object frame in the camera frame
299 cMo = wMc.inverse() * wMo;
300
301 // update new point position and corresponding features
302 for (unsigned i = 0; i < 4; i++) {
303 point[i].track(cMo);
304 // retrieve x,y and Z of the vpPoint structure
305 vpFeatureBuilder::create(p[i], point[i]);
306 }
307 // since vpServo::MEAN interaction matrix is used, we need also to
308 // update the desired features at each iteration
309 pd[0].buildFrom(-0.1, -0.1, 1);
310 pd[1].buildFrom(0.1, -0.1, 1);
311 pd[2].buildFrom(0.1, 0.1, 1);
312 pd[3].buildFrom(-0.1, 0.1, 1);
313
314 if (opt_display) {
315 vpDisplay::display(Iint);
316 vpDisplay::display(Iext);
317 vpServoDisplay::display(task, cam, Iint);
318 externalview.display(Iext, cextMo, cMo, cam, vpColor::green);
319 vpDisplay::flush(Iint);
320 vpDisplay::flush(Iext);
321 }
322
323 // compute the control law
324 v = task.computeControlLaw();
325
326 // send the camera velocity to the controller
328
329 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
330 }
331
332 // Display task information
333 task.print();
334
335 std::cout << "Final robot position with respect to the object frame:\n";
336 cMo.print();
337
338 if (opt_display && opt_click_allowed) {
339 vpDisplay::displayText(Iint, 20, 20, "Click to quit...", vpColor::white);
340 vpDisplay::flush(Iint);
342 }
343 return EXIT_SUCCESS;
344 } catch (const vpException &e) {
345 std::cout << "Catch a ViSP exception: " << e << std::endl;
346 return EXIT_FAILURE;
347 }
348}
349#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
350int main()
351{
352 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
353 return EXIT_SUCCESS;
354}
355#else
356int main()
357{
358 std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..."
359 << std::endl;
360 std::cout << "Tip if you are on a unix-like system:" << std::endl;
361 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
362 std::cout << "Tip if you are on a windows-like system:" << std::endl;
363 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
364 return EXIT_SUCCESS;
365}
366#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor white
Definition vpColor.h:206
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 a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
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
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
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
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.