Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
simulateFourPoints2DPolarCamVelocity.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 visual servoing with visualization.
33 *
34*****************************************************************************/
35
46#include <visp3/core/vpConfig.h>
47#include <visp3/core/vpDebug.h>
48
49#ifdef VISP_HAVE_COIN3D_AND_GUI
50
51#include <visp3/ar/vpSimulator.h>
52#include <visp3/core/vpCameraParameters.h>
53#include <visp3/core/vpHomogeneousMatrix.h>
54#include <visp3/core/vpImage.h>
55#include <visp3/core/vpIoTools.h>
56#include <visp3/core/vpMath.h>
57#include <visp3/core/vpTime.h>
58#include <visp3/io/vpParseArgv.h>
59#include <visp3/robot/vpSimulatorCamera.h>
60#include <visp3/visual_features/vpFeatureBuilder.h>
61#include <visp3/visual_features/vpFeaturePointPolar.h>
62#include <visp3/vs/vpServo.h>
63
64#define GETOPTARGS "di:h"
65#define SAVE 0
66
76void usage(const char *name, const char *badparam, std::string ipath)
77{
78 fprintf(stdout, "\n\
79Simulation Servo 4points.\n\
80 \n\
81SYNOPSIS\n\
82 %s [-i <input image path>] [-d] [-h]\n",
83 name);
84
85 fprintf(stdout, "\n\
86OPTIONS: Default\n\
87 -i <input image path> %s\n\
88 Set image input path.\n\
89 From this path read \"iv/4points.iv\"\n\
90 cad model.\n\
91 Setting the VISP_INPUT_IMAGE_PATH environment\n\
92 variable produces the same behaviour than using\n\
93 this option.\n\
94 \n\
95 -d \n\
96 Disable the image display. This can be useful \n\
97 for automatic tests using crontab under Unix or \n\
98 using the task manager under Windows.\n\
99 \n\
100 -h\n\
101 Print the help.\n\n",
102 ipath.c_str());
103
104 if (badparam)
105 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
106}
107
123bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
124{
125 const char *optarg;
126 int c;
127 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
128
129 switch (c) {
130 case 'i':
131 ipath = optarg;
132 break;
133 case 'd':
134 display = false;
135 break;
136 case 'h':
137 usage(argv[0], NULL, ipath);
138 return false;
139 break;
140
141 default:
142 usage(argv[0], optarg, ipath);
143 return false;
144 break;
145 }
146 }
147
148 if ((c == 1) || (c == -1)) {
149 // standalone param or error
150 usage(argv[0], NULL, ipath);
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
159static void *mainLoop(void *_simu)
160{
161 vpSimulator *simu = static_cast<vpSimulator *>(_simu);
162 simu->initMainApplication();
163
164 vpServo task;
165 vpSimulatorCamera robot;
166
167 float sampling_time = 0.040f; // Sampling period in second
168 robot.setSamplingTime(sampling_time);
170
171 // Sets the initial camera location
172 vpPoseVector vcMo;
173
174 vcMo[0] = 0.;
175 vcMo[1] = 0.;
176 vcMo[2] = 3;
177 vcMo[3] = 0;
178 vcMo[4] = vpMath::rad(0);
179 vcMo[5] = vpMath::rad(90);
180
181 vpHomogeneousMatrix cMo(vcMo);
182 vpHomogeneousMatrix wMo; // Set to identity
183 vpHomogeneousMatrix wMc; // Camera location in world frame
184 wMc = wMo * cMo.inverse();
185 robot.setPosition(wMc);
186 simu->setCameraPosition(cMo);
187
188 simu->getCameraPosition(cMo);
189 wMc = wMo * cMo.inverse();
190 robot.setPosition(wMc);
191
193
194 // Sets the point coordinates in the world frame
195 vpPoint point[4];
196 point[0].setWorldCoordinates(-0.1, -0.1, 0);
197 point[1].setWorldCoordinates(0.1, -0.1, 0);
198 point[2].setWorldCoordinates(0.1, 0.1, 0);
199 point[3].setWorldCoordinates(-0.1, 0.1, 0);
200
201 // Project : computes the point coordinates in the camera frame and its 2D
202 // coordinates
203 for (int i = 0; i < 4; i++) {
204 point[i].changeFrame(cMo); // Compute point coordinates in the camera frame
205 point[i].project(); // Compute desired point doordinates in the camera frame
206 }
207
208 // Sets the desired position of the point
210 for (int i = 0; i < 4; i++)
212 point[i]); // retrieve x,y and Z of the
213 // vpPoint structure to build the
214 // polar coordinates
215
216 std::cout << "s: \n";
217 for (int i = 0; i < 4; i++) {
218 printf("[%d] rho %f theta %f Z %f\n", i, p[i].get_rho(), p[i].get_theta(), p[i].get_Z());
219 }
220
221 // Sets the desired position of the point
222 vcMo[0] = 0;
223 vcMo[1] = 0;
224 vcMo[2] = 1;
225 vcMo[3] = vpMath::rad(0);
226 vcMo[4] = vpMath::rad(0);
227 vcMo[5] = vpMath::rad(0);
228
229 vpHomogeneousMatrix cMod(vcMo);
230
232 vpPoint pointd[4]; // Desired position of the points
233 pointd[0].setWorldCoordinates(-0.1, -0.1, 0);
234 pointd[1].setWorldCoordinates(0.1, -0.1, 0);
235 pointd[2].setWorldCoordinates(0.1, 0.1, 0);
236 pointd[3].setWorldCoordinates(-0.1, 0.1, 0);
237 for (int i = 0; i < 4; i++) {
238 pointd[i].changeFrame(cMod); // Compute desired point doordinates in the camera frame
239 pointd[i].project(); // Compute desired point doordinates in the camera frame
240
241 vpFeatureBuilder::create(pd[i], pointd[i]); // retrieve x,y and Z of the
242 // vpPoint structure to build
243 // the polar coordinates
244 }
245 std::cout << "s*: \n";
246 for (int i = 0; i < 4; i++) {
247 printf("[%d] rho %f theta %f Z %f\n", i, pd[i].get_rho(), pd[i].get_theta(), pd[i].get_Z());
248 }
249
250 // Define the task
251 // We want an eye-in-hand control law
252 // Articular velocity are computed
255
256 // Set the position of the end-effector frame in the camera frame as identity
258 vpVelocityTwistMatrix cVe(cMe);
259 task.set_cVe(cVe);
260
261 // Set the Jacobian (expressed in the end-effector frame)
262 vpMatrix eJe;
263 robot.get_eJe(eJe);
264 task.set_eJe(eJe);
265
266 // We want to see a point on a point
267 for (int i = 0; i < 4; i++)
268 task.addFeature(p[i], pd[i]);
269
270 // Set the gain
271 task.setLambda(1.0);
272
273 // Display task information
274 task.print();
275
276 vpTime::wait(1000); // Sleep 1s
277
278 unsigned int iter = 0;
279 // Visual servo loop
280 while (iter++ < 200) {
281 double t = vpTime::measureTimeMs();
282
283 robot.get_eJe(eJe);
284 task.set_eJe(eJe);
285
286 wMc = robot.getPosition();
287 cMo = wMc.inverse() * wMo;
288 for (int i = 0; i < 4; i++) {
289 point[i].track(cMo);
290 vpFeatureBuilder::create(p[i], point[i]);
291 }
292
295
296 simu->setCameraPosition(cMo);
297
298 if (SAVE == 1) {
299 char name[FILENAME_MAX];
300 snprintf(name, FILENAME_MAX, "/tmp/image.%04u.external.png", iter);
301 std::cout << name << std::endl;
302 simu->write(name);
303 snprintf(name, FILENAME_MAX, "/tmp/image.%04u.internal.png", iter);
304 simu->write(name);
305 }
306
307 vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
308 }
309 // Display task information
310 task.print();
311
312 std::cout << "cMo:\n" << cMo << std::endl;
313 vpPoseVector pose(cMo);
314 std::cout << "final pose:\n" << pose.t() << std::endl;
315
316 simu->closeMainApplication();
317
318 void *a = NULL;
319 return a;
320}
321
322int main(int argc, const char **argv)
323{
324 try {
325 std::string env_ipath;
326 std::string opt_ipath;
327 std::string ipath;
328 std::string filename;
329 bool opt_display = true;
330
331 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
332 // environment variable value
334
335 // Set the default input path
336 if (!env_ipath.empty())
337 ipath = env_ipath;
338
339 // Read the command line options
340 if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
341 return EXIT_FAILURE;
342 }
343
344 // Get the option values
345 if (!opt_ipath.empty())
346 ipath = opt_ipath;
347
348 // Compare ipath and env_ipath. If they differ, we take into account
349 // the input path comming from the command line option
350 if (!opt_ipath.empty() && !env_ipath.empty()) {
351 if (ipath != env_ipath) {
352 std::cout << std::endl << "WARNING: " << std::endl;
353 std::cout << " Since -i <visp image path=" << ipath << "> "
354 << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
355 << " we skip the environment variable." << std::endl;
356 }
357 }
358
359 // Test if an input path is set
360 if (opt_ipath.empty() && env_ipath.empty()) {
361 usage(argv[0], NULL, ipath);
362 std::cerr << std::endl << "ERROR:" << std::endl;
363 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
364 << " environment variable to specify the location of the " << std::endl
365 << " image path where test images are located." << std::endl
366 << std::endl;
367 return EXIT_FAILURE;
368 }
369
372 fMo[2][3] = 0;
373
374 if (opt_display) {
375 vpSimulator simu;
376 simu.initInternalViewer(300, 300);
377 simu.initExternalViewer(300, 300);
378
379 vpTime::wait(1000);
380 simu.setZoomFactor(1.0f);
381
382 // Load the cad model
383 filename = vpIoTools::createFilePath(ipath, "iv/4points.iv");
384 simu.load(filename.c_str());
385
388 simu.initApplication(&mainLoop);
389
390 simu.mainLoop();
391 }
392 return EXIT_SUCCESS;
393 } catch (const vpException &e) {
394 std::cout << "Catch an exception: " << e << std::endl;
395 return EXIT_FAILURE;
396 }
397}
398
399#else
400int main()
401{
402 std::cout << "You do not have Coin3D and SoQT or SoWin or SoXt functionalities enabled..." << std::endl;
403 std::cout << "Tip:" << std::endl;
404 std::cout
405 << "- Install Coin3D and SoQT or SoWin or SoXt, configure ViSP again using cmake and build again this example"
406 << std::endl;
407 return EXIT_SUCCESS;
408}
409#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
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
static std::string getViSPImagesDataPath()
static std::string createFilePath(const std::string &parent, const std::string &child)
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 changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition vpPoint.cpp:236
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
Implementation of a pose vector and operations on poses.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ CAMERA_FRAME
Definition vpRobot.h:80
void setMaxTranslationVelocity(double maxVt)
Definition vpRobot.cpp:236
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 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.
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition vpSimulator.h:99
void load(const char *file_name)
load an iv file
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
virtual void mainLoop()
activate the mainloop
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
void initMainApplication()
perform some initialization in the main program thread
void initApplication(void *(*start_routine)(void *))
begin the main program
void getCameraPosition(vpHomogeneousMatrix &_cMf)
get the camera position (from an homogeneous matrix)
void setZoomFactor(float zoom)
set the size of the camera/frame
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
void write(const char *fileName)
virtual void initInternalViewer(unsigned int nlig, unsigned int ncol)
initialize the camera view
void closeMainApplication()
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()