Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuViper850FourPoints2DCamVelocity.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(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) && \
57 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) && \
58 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
59
60// We need to use threading capabilities. Thus on Unix-like
61// platforms, the libpthread third-party library need to be
62// installed. On Windows, we use the native threading capabilities.
63
64#include <stdio.h>
65#include <stdlib.h>
66
67#include <visp3/core/vpCameraParameters.h>
68#include <visp3/core/vpHomogeneousMatrix.h>
69#include <visp3/core/vpImage.h>
70#include <visp3/core/vpImagePoint.h>
71#include <visp3/core/vpIoTools.h>
72#include <visp3/core/vpMath.h>
73#include <visp3/core/vpMeterPixelConversion.h>
74#include <visp3/gui/vpDisplayGDI.h>
75#include <visp3/gui/vpDisplayGTK.h>
76#include <visp3/gui/vpDisplayX.h>
77#include <visp3/io/vpParseArgv.h>
78#include <visp3/robot/vpSimulatorViper850.h>
79#include <visp3/visual_features/vpFeatureBuilder.h>
80#include <visp3/visual_features/vpFeaturePoint.h>
81#include <visp3/vs/vpServo.h>
82
83// List of allowed command line options
84#define GETOPTARGS "cdh"
85
86void usage(const char *name, const char *badparam);
87bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
88
97void usage(const char *name, const char *badparam)
98{
99 fprintf(stdout, "\n\
100Tests a control law with the following characteristics:\n\
101- eye-in-hand control\n\
102- articular velocity are computed\n\
103- servo on 4 points,\n\
104- internal and external camera view displays.\n\
105 \n\
106SYNOPSIS\n\
107 %s [-c] [-d] [-h]\n",
108 name);
109
110 fprintf(stdout, "\n\
111OPTIONS: Default\n\
112 -c\n\
113 Disable the mouse click. Useful to automate the \n\
114 execution of this program without human intervention.\n\
115 \n\
116 -d \n\
117 Turn off the display.\n\
118 \n\
119 -h\n\
120 Print the help.\n");
121
122 if (badparam)
123 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
124}
137bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
138{
139 const char *optarg_;
140 int c;
141 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
142
143 switch (c) {
144 case 'c':
145 click_allowed = false;
146 break;
147 case 'd':
148 display = false;
149 break;
150 case 'h':
151 usage(argv[0], NULL);
152 return false;
153 break;
154
155 default:
156 usage(argv[0], optarg_);
157 return false;
158 break;
159 }
160 }
161
162 if ((c == 1) || (c == -1)) {
163 // standalone param or error
164 usage(argv[0], NULL);
165 std::cerr << "ERROR: " << std::endl;
166 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
167 return false;
168 }
169
170 return true;
171}
172
173int main(int argc, const char **argv)
174{
175 try {
176 bool opt_click_allowed = true;
177 bool opt_display = true;
178
179 // Read the command line options
180 if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
181 return EXIT_FAILURE;
182 }
183
184 // We open two displays, one for the internal camera view, the other one for
185 // the external view, using either X11, GTK or GDI.
186#if defined(VISP_HAVE_X11)
187 vpDisplayX displayInt;
188#elif defined(VISP_HAVE_GDI)
189 vpDisplayGDI displayInt;
190#elif defined(HAVE_OPENCV_HIGHGUI)
191 vpDisplayOpenCV displayInt;
192#endif
193
194 // open a display for the visualization
195
196 vpImage<unsigned char> Iint(480, 640, 255);
197
198 if (opt_display) {
199 displayInt.init(Iint, 700, 0, "Internal view");
200 }
201
202 vpServo task;
203
204 std::cout << std::endl;
205 std::cout << "----------------------------------------------" << std::endl;
206 std::cout << " Test program for vpServo " << std::endl;
207 std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
208 std::cout << " Simulation " << std::endl;
209 std::cout << " task : servo 4 points " << std::endl;
210 std::cout << "----------------------------------------------" << std::endl;
211 std::cout << std::endl;
212
213 // sets the initial camera location
214 vpHomogeneousMatrix cMo(-0.05, -0.05, 0.7, vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
215
216 // sets the point coordinates in the object frame
217 vpPoint point[4];
218 point[0].setWorldCoordinates(-0.045, -0.045, 0);
219 point[3].setWorldCoordinates(-0.045, 0.045, 0);
220 point[2].setWorldCoordinates(0.045, 0.045, 0);
221 point[1].setWorldCoordinates(0.045, -0.045, 0);
222
223 // computes the point coordinates in the camera frame and its 2D
224 // coordinates
225 for (unsigned int i = 0; i < 4; i++)
226 point[i].track(cMo);
227
228 // sets the desired position of the point
229 vpFeaturePoint p[4];
230 for (unsigned int i = 0; i < 4; i++)
231 vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
232
233 // sets the desired position of the feature point s*
234 vpFeaturePoint pd[4];
235
236 // Desired pose
238
239 // Projection of the points
240 for (unsigned int i = 0; i < 4; i++)
241 point[i].track(cdMo);
242
243 for (unsigned int i = 0; i < 4; i++)
244 vpFeatureBuilder::create(pd[i], point[i]);
245
246 // define the task
247 // - we want an eye-in-hand control law
248 // - articular velocity are computed
251
252 // - we want to see a point on a point
253 for (unsigned int i = 0; i < 4; i++)
254 task.addFeature(p[i], pd[i]);
255
256 // set the gain
257 task.setLambda(0.8);
258
259 // Declaration of the robot
260 vpSimulatorViper850 robot(opt_display);
261
262 // Initialise the robot and especially the camera
265
266 // Initialise the object for the display part
268
269 // Initialise the position of the object relative to the pose of the
270 // robot's camera
271 robot.initialiseObjectRelativeToCamera(cMo);
272
273 // Set the desired position (for the display part)
274 robot.setDesiredCameraPosition(cdMo);
275
276 // Get the internal robot's camera parameters
278 robot.getCameraParameters(cam, Iint);
279
280 if (opt_display) {
281 // Get the internal view
282 vpDisplay::display(Iint);
283 robot.getInternalView(Iint);
284 vpDisplay::flush(Iint);
285 }
286
287 // Display task information
288 task.print();
289
290 unsigned int iter = 0;
291 // loop
292 while (iter++ < 500) {
293 std::cout << "---------------------------------------------" << iter << std::endl;
294 vpColVector v;
295
296 // Get the Time at the beginning of the loop
297 double t = vpTime::measureTimeMs();
298
299 // Get the current pose of the camera
300 cMo = robot.get_cMo();
301
302 if (iter == 1) {
303 std::cout << "Initial robot position with respect to the object frame:\n";
304 cMo.print();
305 }
306
307 // new point position
308 for (unsigned int i = 0; i < 4; i++) {
309 point[i].track(cMo);
310 // retrieve x,y and Z of the vpPoint structure
311 vpFeatureBuilder::create(p[i], point[i]);
312 }
313
314 if (opt_display) {
315 // Get the internal view and display it
316 vpDisplay::display(Iint);
317 robot.getInternalView(Iint);
318 vpDisplay::flush(Iint);
319 }
320
321 if (opt_display && opt_click_allowed && iter == 1) {
322 // suppressed for automate test
323 std::cout << "Click in the internal view window to continue..." << std::endl;
325 }
326
327 // compute the control law
328 v = task.computeControlLaw();
329
330 // send the camera velocity to the controller
332
333 std::cout << "|| s - s* || " << (task.getError()).sumSquare() << std::endl;
334
335 // The main loop has a duration of 10 ms at minimum
336 vpTime::wait(t, 10);
337 }
338
339 // Display task information
340 task.print();
341
342 std::cout << "Final robot position with respect to the object frame:\n";
343 cMo.print();
344
345 if (opt_display && opt_click_allowed) {
346 // suppressed for automate test
347 std::cout << "Click in the internal view window to end..." << std::endl;
349 }
350 return EXIT_SUCCESS;
351 } catch (const vpException &e) {
352 std::cout << "Catch a ViSP exception: " << e << std::endl;
353 return EXIT_FAILURE;
354 }
355}
356#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
357int main()
358{
359 std::cout << "You do not have X11, or GDI (Graphical Device Interface) of OpenCV functionalities to display images..."
360 << std::endl;
361 std::cout << "Tip if you are on a unix-like system:" << std::endl;
362 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
363 std::cout << "Tip if you are on a windows-like system:" << std::endl;
364 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
365 return EXIT_SUCCESS;
366}
367#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
368int main()
369{
370 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
371 return EXIT_SUCCESS;
372}
373#else
374int main()
375{
376 std::cout << "You do not have threading capabilities" << std::endl;
377 std::cout << "Tip:" << std::endl;
378 std::cout << "- Install pthread, configure again ViSP using cmake and build again this example" << std::endl;
379 return EXIT_SUCCESS;
380}
381#endif
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
Display for windows using GDI (available on any windows 32 platform).
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)
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.
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double rad(double deg)
Definition vpMath.h:116
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
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition vpRobot.h:80
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
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
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
Simulator of Irisa's Viper S850 robot named Viper850.
@ TOOL_PTGREY_FLEA2_CAMERA
Definition vpViper850.h:127
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)