Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoBiclopsPoint2DArtVelocity.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 * tests the control law
33 * eye-in-hand control
34 * velocity computed in articular
35 *
36*****************************************************************************/
37
56#include <signal.h>
57#include <stdlib.h>
58#include <visp3/core/vpConfig.h>
59#include <visp3/core/vpDebug.h> // Debug trace
60#include <visp3/core/vpTime.h>
61#if (defined(VISP_HAVE_BICLOPS) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW)))
62
63#ifdef VISP_HAVE_PTHREAD
64#include <pthread.h>
65#endif
66
67#include <visp3/core/vpDisplay.h>
68#include <visp3/core/vpImage.h>
69#include <visp3/gui/vpDisplayGDI.h>
70#include <visp3/gui/vpDisplayGTK.h>
71#include <visp3/gui/vpDisplayX.h>
72#include <visp3/sensor/vp1394TwoGrabber.h>
73#include <visp3/sensor/vpDirectShowGrabber.h>
74
75#include <visp3/blob/vpDot.h>
76#include <visp3/core/vpHomogeneousMatrix.h>
77#include <visp3/core/vpIoTools.h>
78#include <visp3/core/vpMath.h>
79#include <visp3/core/vpPoint.h>
80#include <visp3/io/vpParseArgv.h>
81#include <visp3/robot/vpRobotBiclops.h>
82#include <visp3/visual_features/vpFeatureBuilder.h>
83#include <visp3/visual_features/vpFeaturePoint.h>
84#include <visp3/vs/vpServo.h>
85#include <visp3/vs/vpServoDisplay.h>
86
87// Exception
88#include <visp3/core/vpException.h>
89
90#ifdef VISP_HAVE_PTHREAD
91pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
92#endif
93
94void signalCtrC(int /* signumber */)
95{
96#ifdef VISP_HAVE_PTHREAD
97 pthread_mutex_unlock(&mutexEndLoop);
98#endif
99 vpTime::wait(10);
100 vpTRACE("Ctrl-C pressed...");
101}
102
103// List of allowed command line options
104#define GETOPTARGS "c:d:h"
105
117void usage(const char *name, const char *badparam, std::string &conf, std::string &debugdir, std::string &user)
118{
119 fprintf(stdout, "\n\
120 Example of eye-in-hand control law. We control here a real robot, the biclops\n\
121 robot (pan-tilt head provided by Traclabs). The velocity is\n\
122 computed in articular. The visual feature is the center of gravity of a\n\
123 point.\n\
124\n\
125SYNOPSIS\n\
126 %s [-c <Biclops configuration file>] [-d <debug file directory>] [-h]\n",
127 name);
128
129 fprintf(stdout, "\n\
130OPTIONS: Default\n\
131 -c <Biclops configuration file> %s\n\
132 Sets the biclops robot configuration file.\n\n\
133 -d <debug file directory> %s\n\
134 Sets the debug file directory.\n\
135 From this directory, creates the\"%s\"\n\
136 subdirectory depending on the username, where\n\
137 it writes biclops.txt file.\n",
138 conf.c_str(), debugdir.c_str(), user.c_str());
139
140 if (badparam) {
141 fprintf(stderr, "ERROR: \n");
142 fprintf(stderr, "\nBad parameter [%s]\n", badparam);
143 }
144}
158bool getOptions(int argc, const char **argv, std::string &conf, std::string &debugdir, std::string &user)
159{
160 const char *optarg_;
161 int c;
162 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
163
164 switch (c) {
165 case 'c':
166 conf = optarg_;
167 break;
168 case 'd':
169 debugdir = optarg_;
170 break;
171 case 'h':
172 usage(argv[0], NULL, conf, debugdir, user);
173 return false;
174 break;
175
176 default:
177 usage(argv[0], optarg_, conf, debugdir, user);
178 return false;
179 break;
180 }
181 }
182
183 if ((c == 1) || (c == -1)) {
184 // standalone param or error
185 usage(argv[0], NULL, conf, debugdir, user);
186 std::cerr << "ERROR: " << std::endl;
187 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
188 return false;
189 }
190
191 return true;
192}
193
194int main(int argc, const char **argv)
195{
196 std::cout << std::endl;
197 std::cout << "-------------------------------------------------------" << std::endl;
198 std::cout << " Test program for vpServo " << std::endl;
199 std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
200 std::cout << " Simulation " << std::endl;
201 std::cout << " task : servo a point " << std::endl;
202 std::cout << "-------------------------------------------------------" << std::endl;
203 std::cout << std::endl;
204
205 try {
206
207#ifdef VISP_HAVE_PTHREAD
208 pthread_mutex_lock(&mutexEndLoop);
209#endif
210 signal(SIGINT, &signalCtrC);
211
212 // default unix configuration file path
213 std::string opt_conf = "/usr/share/BiclopsDefault.cfg";
214
215 std::string username;
216 std::string debugdir;
217 std::string opt_debugdir;
218
219// Set the default output path
220#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
221 opt_debugdir = "/tmp";
222#elif defined(_WIN32)
223 opt_debugdir = "C:/temp";
224#endif
225
226 // Get the user login name
227 vpIoTools::getUserName(username);
228
229 // Read the command line options
230 if (getOptions(argc, argv, opt_conf, opt_debugdir, username) == false) {
231 return EXIT_FAILURE;
232 }
233
234 // Get the option value
235 if (!opt_debugdir.empty())
236 debugdir = opt_debugdir;
237
238 // Append to the output path string, the login name of the user
239 std::string dirname = debugdir + "/" + username;
240
241 // Test if the output path exist. If no try to create it
242 if (vpIoTools::checkDirectory(dirname) == false) {
243 try {
244 // Create the dirname
246 } catch (...) {
247 usage(argv[0], NULL, opt_conf, debugdir, username);
248 std::cerr << std::endl << "ERROR:" << std::endl;
249 std::cerr << " Cannot create " << dirname << std::endl;
250 std::cerr << " Check your -d " << debugdir << " option " << std::endl;
251 return EXIT_FAILURE;
252 }
253 }
254
255 // Create the debug file: debugdir/$user/biclops.txt
256 std::string filename;
257 filename = debugdir + "/biclops.txt";
258 FILE *fd = fopen(filename.c_str(), "w");
259
260 vpRobotBiclops robot(opt_conf.c_str());
261 robot.setDenavitHartenbergModel(vpBiclops::DH2);
262
263 {
264 vpColVector q(2);
265 q = 0;
267 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
268 }
269
271
272#if defined VISP_HAVE_DC1394
274#elif defined VISP_HAVE_DIRECTSHOW
276#endif
277
278 g.open(I);
279
280 try {
281 g.acquire(I);
282 } catch (...) {
283 vpERROR_TRACE(" Error caught");
284 return EXIT_FAILURE;
285 }
286
287// We open a window using either X11 or GTK or GDI.
288// Its size is automatically defined by the image (I) size
289#if defined(VISP_HAVE_X11)
290 vpDisplayX display(I, 100, 100, "Display X...");
291#elif defined(VISP_HAVE_GTK)
292 vpDisplayGTK display(I, 100, 100, "Display GTK...");
293#elif defined(_WIN32)
294 vpDisplayGDI display(I, 100, 100, "Display GDI...");
295#endif
296
297 try {
300 } catch (...) {
301 vpERROR_TRACE(" Error caught");
302 return EXIT_FAILURE;
303 }
304
305 vpServo task;
306
307 vpDot dot;
308
309 try {
310 std::cout << "Click on a dot to initialize the tracking..." << std::endl;
311 dot.setGraphics(true);
312 dot.initTracking(I);
313 dot.track(I);
314 vpERROR_TRACE("after dot.initTracking(I) ");
315 } catch (...) {
316 vpERROR_TRACE(" Error caught");
317 return EXIT_FAILURE;
318 }
319
321
322 // sets the current position of the visual feature
324 vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
325
326 p.set_Z(1);
327 // sets the desired position of the visual feature
329 pd.buildFrom(0, 0, 1);
330
331 // define the task
332 // - we want an eye-in-hand control law
333 // - articular velocity are computed
336
337 vpTRACE("Set the position of the end-effector frame in the camera frame");
339 // robot.get_cMe(cMe) ;
340
342 robot.get_cVe(cVe);
343 std::cout << cVe << std::endl;
344 task.set_cVe(cVe);
345
346 std::cout << "Click in the image to start the servoing..." << std::endl;
348
349 // Set the Jacobian (expressed in the end-effector frame)
350 vpMatrix eJe;
351 robot.get_eJe(eJe);
352 task.set_eJe(eJe);
353
354 // we want to see a point on a point
355 task.addFeature(p, pd);
356
357 // set the gain
358 task.setLambda(0.2);
359
360 // Display task information
361 task.print();
362
364
365 unsigned int iter = 0;
366 vpTRACE("\t loop");
367#ifdef VISP_HAVE_PTHREAD
368 while (0 != pthread_mutex_trylock(&mutexEndLoop))
369#else
370 for (;;)
371#endif
372 {
373 std::cout << "---------------------------------------------" << iter << std::endl;
374
375 g.acquire(I);
377
378 dot.track(I);
379
380 vpFeatureBuilder::create(p, cam, dot);
381
382 // get the jacobian
383 robot.get_eJe(eJe);
384 task.set_eJe(eJe);
385
386 // std::cout << (vpMatrix)cVe*eJe << std::endl ;
387
388 vpColVector v;
389 v = task.computeControlLaw();
390
391 vpServoDisplay::display(task, cam, I);
393
394 std::cout << "v: " << v.t();
396
397 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
398
399 {
400 vpColVector s_minus_sStar(2);
401 s_minus_sStar = task.s - task.sStar;
402 fprintf(fd, "%f %f %f %f %f\n", v[0], v[1], s_minus_sStar[0], s_minus_sStar[1], (task.getError()).sumSquare());
403 }
404 }
405
406 std::cout << "Display task information " << std::endl;
407 task.print();
408
409 fclose(fd);
410
411 return EXIT_SUCCESS;
412 } catch (const vpException &e) {
413 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
414 return EXIT_FAILURE;
415 }
416}
417
418#else
419int main()
420{
421 std::cout << "You do not have an biclops PT robot connected to your computer..." << std::endl;
422 return EXIT_SUCCESS;
423}
424#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
void open(vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
vpRowVector t() const
class for windows direct show devices
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...
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)
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage.
Definition vpDot.h:112
void initTracking(const vpImage< unsigned char > &I)
Definition vpDot.cpp:617
void setGraphics(bool activate)
Definition vpDot.h:357
void track(const vpImage< unsigned char > &I)
Definition vpDot.cpp:757
error that can be emitted by ViSP classes.
Definition vpException.h:59
const char * getMessage() const
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 set_Z(double Z)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
Definition vpImage.h:135
static bool checkDirectory(const std::string &dirname)
static std::string getUserName()
static void makeDirectory(const std::string &dirname)
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Interface for the biclops, pan, tilt head control.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
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 sStar
Definition vpServo.h:563
vpColVector s
Definition vpServo.h:559
vpColVector getError() const
Definition vpServo.h:276
@ PSEUDO_INVERSE
Definition vpServo.h:199
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
vpVelocityTwistMatrix get_cVe() const
Definition vpUnicycle.h:79
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388
VISP_EXPORT int wait(double t0, double t)