Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuAfma6FourPoints2DCamVelocity.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 cartesian
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/vpSimulatorAfma6.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 vpImage<unsigned char> Iint(480, 640, 255);
195
196 if (opt_display) {
197 // open a display for the visualization
198 displayInt.init(Iint, 700, 0, "Internal view");
199 }
200
201 vpServo task;
202
203 std::cout << std::endl;
204 std::cout << "----------------------------------------------" << std::endl;
205 std::cout << " Test program for vpServo " << std::endl;
206 std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
207 std::cout << " Simulation " << std::endl;
208 std::cout << " task : servo 4 points " << std::endl;
209 std::cout << "----------------------------------------------" << std::endl;
210 std::cout << std::endl;
211
212 // sets the initial camera location
213 vpHomogeneousMatrix cMo(-0.05, -0.05, 0.7, vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
214
215 // sets the point coordinates in the object frame
216 vpPoint point[4];
217 point[0].setWorldCoordinates(-0.045, -0.045, 0);
218 point[3].setWorldCoordinates(-0.045, 0.045, 0);
219 point[2].setWorldCoordinates(0.045, 0.045, 0);
220 point[1].setWorldCoordinates(0.045, -0.045, 0);
221
222 // computes the point coordinates in the camera frame and its 2D
223 // coordinates
224 for (unsigned int i = 0; i < 4; i++)
225 point[i].track(cMo);
226
227 // sets the desired position of the point
228 vpFeaturePoint p[4];
229 for (unsigned int i = 0; i < 4; i++)
230 vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
231
232 // sets the desired position of the feature point s*
233 vpFeaturePoint pd[4];
234
235 // Desired pose
237
238 // Projection of the points
239 for (unsigned int i = 0; i < 4; i++)
240 point[i].track(cdMo);
241
242 for (unsigned int i = 0; i < 4; i++)
243 vpFeatureBuilder::create(pd[i], point[i]);
244
245 // define the task
246 // - we want an eye-in-hand control law
247 // - articular velocity are computed
250
251 // we want to see a point on a point
252 for (unsigned int i = 0; i < 4; i++)
253 task.addFeature(p[i], pd[i]);
254
255 // set the gain
256 task.setLambda(0.8);
257
258 // Declaration of the robot
259 vpSimulatorAfma6 robot(opt_display);
260
261 // Initialise the robot and especially the camera
264
265 // Initialise the object for the display part*/
267
268 // Initialise the position of the object relative to the pose of the
269 // robot's camera
270 robot.initialiseObjectRelativeToCamera(cMo);
271
272 // Set the desired position (for the displaypart)
273 robot.setDesiredCameraPosition(cdMo);
274
275 // Get the internal robot's camera parameters
277 robot.getCameraParameters(cam, Iint);
278
279 if (opt_display) {
280 // Get the internal view
281 vpDisplay::display(Iint);
282 robot.getInternalView(Iint);
283 vpDisplay::flush(Iint);
284 }
285
286 // Display task information
287 task.print();
288
289 unsigned int iter = 0;
290 vpTRACE("\t loop");
291 while (iter++ < 500) {
292 std::cout << "---------------------------------------------" << iter << std::endl;
293 vpColVector v;
294
295 // Get the Time at the beginning of the loop
296 double t = vpTime::measureTimeMs();
297
298 // Get the current pose of the camera
299 cMo = robot.get_cMo();
300
301 if (iter == 1) {
302 std::cout << "Initial robot position with respect to the object frame:\n";
303 cMo.print();
304 }
305
306 // new point position
307 for (unsigned int i = 0; i < 4; i++) {
308 point[i].track(cMo);
309 // retrieve x,y and Z of the vpPoint structure
310 vpFeatureBuilder::create(p[i], point[i]);
311 }
312
313 if (opt_display) {
314 // Get the internal view and display it
315 vpDisplay::display(Iint);
316 robot.getInternalView(Iint);
317 vpDisplay::flush(Iint);
318 }
319
320 if (opt_display && opt_click_allowed && iter == 1) {
321 // suppressed for automate test
322 std::cout << "Click in the internal view window to continue..." << std::endl;
324 }
325
326 // compute the control law
327 v = task.computeControlLaw();
328
329 // send the camera velocity to the controller
331
332 std::cout << "|| s - s* || " << (task.getError()).sumSquare() << std::endl;
333
334 // The main loop has a duration of 10 ms at minimum
335 vpTime::wait(t, 10);
336 }
337
338 // Display task information
339 task.print();
340
341 std::cout << "Final robot position with respect to the object frame:\n";
342 cMo.print();
343
344 if (opt_display && opt_click_allowed) {
345 // suppressed for automate test
346 std::cout << "Click in the internal view window to end..." << std::endl;
348 }
349 return EXIT_SUCCESS;
350 }
351 catch (const vpException &e) {
352 std::cout << "Catch a ViSP exception: " << e << std::endl;
353 return EXIT_FAILURE;
354 }
355 return EXIT_SUCCESS;
356}
357#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
358int main()
359{
360 std::cout << "You do not have X11, or GDI (Graphical Device Interface) of OpenCV functionalities to display images..."
361 << std::endl;
362 std::cout << "Tip if you are on a unix-like system:" << std::endl;
363 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
364 std::cout << "Tip if you are on a windows-like system:" << std::endl;
365 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
366 return EXIT_SUCCESS;
367}
368#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
369int main()
370{
371 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
372 return EXIT_SUCCESS;
373}
374#else
375int main()
376{
377 std::cout << "You do not have threading capabilities" << std::endl;
378 std::cout << "Tip:" << std::endl;
379 std::cout << "- Install pthread, configure again ViSP using cmake and build again this example" << std::endl;
380 return EXIT_SUCCESS;
381}
382#endif
@ TOOL_CCMOP
Definition vpAfma6.h:124
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 gantry robot named Afma6.
#define vpTRACE
Definition vpDebug.h:411
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)