Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma4Point2DCamVelocityKalman.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 the camera frame
35 *
36*****************************************************************************/
37
52#include <stdlib.h>
53#include <visp3/core/vpConfig.h>
54#include <visp3/core/vpDebug.h> // Debug trace
55#if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
56
57#include <visp3/core/vpDisplay.h>
58#include <visp3/core/vpImage.h>
59#include <visp3/gui/vpDisplayGTK.h>
60#include <visp3/gui/vpDisplayOpenCV.h>
61#include <visp3/gui/vpDisplayX.h>
62#include <visp3/sensor/vp1394TwoGrabber.h>
63
64#include <visp3/blob/vpDot2.h>
65#include <visp3/core/vpException.h>
66#include <visp3/core/vpHomogeneousMatrix.h>
67#include <visp3/core/vpIoTools.h>
68#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
69#include <visp3/core/vpMath.h>
70#include <visp3/core/vpPoint.h>
71#include <visp3/io/vpParseArgv.h>
72#include <visp3/robot/vpRobotAfma4.h>
73#include <visp3/visual_features/vpFeatureBuilder.h>
74#include <visp3/visual_features/vpFeaturePoint.h>
75#include <visp3/vs/vpAdaptiveGain.h>
76#include <visp3/vs/vpServo.h>
77#include <visp3/vs/vpServoDisplay.h>
78
79// List of allowed command line options
80#define GETOPTARGS "hK:l:"
81
82typedef enum { K_NONE, K_VELOCITY, K_ACCELERATION } KalmanType;
83
93void usage(const char *name, const char *badparam, KalmanType &kalman)
94{
95 fprintf(stdout, "\n\
96Tests a control law with the following characteristics:\n\
97- eye-in-hand control\n\
98- camera velocity are computed\n\
99- servo on 1 points.\n\
100- Kalman filtering\n\
101 \n\
102SYNOPSIS\n\
103 %s [-K <0|1|2|3>] [-h]\n",
104 name);
105
106 fprintf(stdout, "\n\
107OPTIONS: Default\n\
108 -l <%%f> \n\
109 Set the constant gain. By default adaptive gain. \n\
110 \n\
111 -K <0|1|2> %d\n\
112 Kalman filtering:\n\
113 0: none\n\
114 1: velocity model\n\
115 2: acceleration model\n\
116 \n\
117 -h\n\
118 Print the help.\n",
119 (int)kalman);
120
121 if (badparam) {
122 fprintf(stderr, "ERROR: \n");
123 fprintf(stderr, "\nBad parameter [%s]\n", badparam);
124 }
125}
126
141bool getOptions(int argc, const char **argv, KalmanType &kalman, bool &doAdaptativeGain,
142 vpAdaptiveGain &lambda) // gain lambda
143{
144 const char *optarg;
145 int c;
146 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
147
148 switch (c) {
149 case 'K':
150 kalman = (KalmanType)atoi(optarg);
151 break;
152 case 'l':
153 doAdaptativeGain = false;
154 lambda.initFromConstant(atof(optarg));
155 break;
156 case 'h':
157 usage(argv[0], NULL, kalman);
158 return false;
159 break;
160
161 default:
162 usage(argv[0], optarg, kalman);
163 return false;
164 break;
165 }
166 }
167
168 if ((c == 1) || (c == -1)) {
169 // standalone param or error
170 usage(argv[0], NULL, kalman);
171 std::cerr << "ERROR: " << std::endl;
172 std::cerr << " Bad argument " << optarg << std::endl << std::endl;
173 return false;
174 }
175
176 return true;
177}
178
179int main(int argc, const char **argv)
180{
181 try {
182 KalmanType opt_kalman = K_NONE;
183 vpAdaptiveGain lambda; // Gain de la commande
184 bool doAdaptativeGain = true; // Compute adaptative gain
185 lambda.initStandard(4, 0.2, 40);
186 int opt_cam_frequency = 60; // 60 Hz
187
188 // Read the command line options
189 if (getOptions(argc, argv, opt_kalman, doAdaptativeGain, lambda) == false) {
190 return EXIT_FAILURE;
191 }
192
193 // Log file creation in /tmp/$USERNAME/log.dat
194 // This file contains by line:
195 // - the 6 computed cam velocities (m/s, rad/s) to achieve the task
196 // - the 6 mesured joint velocities (m/s, rad/s)
197 // - the 6 mesured joint positions (m, rad)
198 // - the 2 values of s - s*
199 std::string username;
200 // Get the user login name
201 vpIoTools::getUserName(username);
202
203 // Create a log filename to save velocities...
204 std::string logdirname;
205 logdirname = "/tmp/" + username;
206
207 // Test if the output path exist. If no try to create it
208 if (vpIoTools::checkDirectory(logdirname) == false) {
209 try {
210 // Create the dirname
211 vpIoTools::makeDirectory(logdirname);
212 } catch (...) {
213 std::cerr << std::endl << "ERROR:" << std::endl;
214 std::cerr << " Cannot create " << logdirname << std::endl;
215 return EXIT_FAILURE;
216 }
217 }
218 std::string logfilename;
219 logfilename = logdirname + "/log.dat";
220
221 // Open the log file name
222 std::ofstream flog(logfilename.c_str());
223
224 vpServo task;
225
227 vp1394TwoGrabber g(false);
229 switch (opt_cam_frequency) {
230 case 15:
231 g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
232 break;
233 case 30:
234 g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
235 break;
236 case 60:
237 g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
238 break;
239 }
240 g.open(I);
241
242 for (int i = 0; i < 10; i++) // 10 acquisition to warm up the camera
243 g.acquire(I);
244
245#ifdef VISP_HAVE_X11
246 vpDisplayX display(I, 100, 100, "Current image");
247#elif defined(HAVE_OPENCV_HIGHGUI)
248 vpDisplayOpenCV display(I, 100, 100, "Current image");
249#elif defined(VISP_HAVE_GTK)
250 vpDisplayGTK display(I, 100, 100, "Current image");
251#endif
252
255
256 std::cout << std::endl;
257 std::cout << "-------------------------------------------------------" << std::endl;
258 std::cout << "Test program for target motion compensation using a Kalman "
259 "filter "
260 << std::endl;
261 std::cout << "Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
262 std::cout << "Task : servo a point \n" << std::endl;
263
264 // Kalman filtering
265 switch (opt_kalman) {
266 case K_NONE:
267 std::cout << "Servo with no target motion compensation (see -K option)\n";
268 break;
269 case K_VELOCITY:
270 std::cout << "Servo with target motion compensation using a Kalman filter\n"
271 << "with constant velocity modelization (see -K option)\n";
272 break;
273 case K_ACCELERATION:
274 std::cout << "Servo with target motion compensation using a Kalman filter\n"
275 << "with constant acceleration modelization (see -K option)\n";
276 break;
277 }
278 std::cout << "-------------------------------------------------------" << std::endl;
279 std::cout << std::endl;
280
281 vpDot2 dot;
282
283 std::cout << "Click on the dot..." << std::endl;
284 dot.setGraphics(true);
285 dot.initTracking(I);
286 vpImagePoint cog;
287 cog = dot.getCog();
290
291 vpRobotAfma4 robot;
292
293 double px = 1000;
294 double py = 1000;
295 double u0 = I.getWidth() / 2.;
296 double v0 = I.getHeight() / 2.;
297
298 vpCameraParameters cam(px, py, u0, v0);
299
300 // Sets the current position of the visual feature
302 vpFeatureBuilder::create(p, cam, dot);
303
304 // Sets the desired position of the visual feature
306 pd.buildFrom(0, 0, 1);
307
308 // Define the task
309 // - we want an eye-in-hand control law
310 // - robot is controlled in the camera frame
312
313 // - we want to see a point on a point
314 std::cout << std::endl;
315 task.addFeature(p, pd);
316
317 // - set the gain
318 task.setLambda(lambda);
319
320 // Display task information
321 // task.print() ;
322
323 //--------------------------------------------------------------------------
325 //--------------------------------------------------------------------------
326
329
330 // Initialize the kalman filter
331 unsigned int nsignal = 2; // The two values of dedt
332 double rho = 0.3;
333 vpColVector sigma_state;
334 vpColVector sigma_measure(nsignal);
335 unsigned int state_size = 0; // Kalman state vector size
336
337 switch (opt_kalman) {
338 case K_VELOCITY: {
339 // Set the constant velocity state model used for the filtering
341 state_size = kalman.getStateSize();
342 sigma_state.resize(state_size * nsignal);
343 sigma_state = 0.00001; // Same state variance for all signals
344 sigma_measure = 0.05; // Same measure variance for all the signals
345 double dummy = 0; // non used parameter dt for the velocity state model
346 kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
347
348 break;
349 }
350 case K_ACCELERATION: {
351 // Set the constant acceleration state model used for the filtering
353 state_size = kalman.getStateSize();
354 sigma_state.resize(state_size * nsignal);
355 sigma_state = 0.00001; // Same variance for all the signals
356 sigma_measure = 0.05; // Same measure variance for all the signals
357 double dt = 1. / opt_cam_frequency;
358 kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
359 break;
360 }
361 default:
362 break;
363 }
364
366
367 int iter = 0;
368
369 double t_1, Tv_0;
370 vpColVector vm(6), vm_0(6);
371 vpColVector v(6), v1(6), v2(6); // robot velocities
372 // task error
373 vpColVector err(2), err_0(2), err_1(2);
374 vpColVector dedt_filt(2), dedt_mes(2);
375
376 t_1 = vpTime::measureTimeMs(); // t_1: time at previous iter
377
378 Tv_0 = 0;
379
380 //
381 // Warning: In all varaible names,
382 // _0 means the value for the current iteration (t=0)
383 // _1 means the value for the previous iteration (t=-1)
384 // _2 means the value for the previous previous iteration (t=-2)
385 //
386 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
387 for (;;) {
388 double t_0 = vpTime::measureTimeMs(); // t_0: current time
389 // Temps de la boucle d'asservissement
390 double Tv = (double)(t_0 - t_1) / 1000.0; // temps d'une iteration en s
391 // !
392 // std::cout << "time iter : " << Tv << std::endl;
393
394 // Update time for next iteration
395 t_1 = t_0;
396
398
399 // Acquire a new image from the camera
400 g.acquire(I);
401
402 // Display this image
404
405 // Achieve the tracking of the dot in the image
406 dot.track(I);
407 vpImagePoint cog = dot.getCog();
408
409 // Display a green cross at the center of gravity position in the image
411
412 // Update the point feature from the dot location
413 vpFeatureBuilder::create(p, cam, dot);
414
415 //----------------------------------------------------------------------
417 //----------------------------------------------------------------------
418 vm_0 = vm;
419
420 // Update current loop time and previous one
421 double Tv_1 = Tv_0;
422 Tv_0 = Tv;
423
424 // Compute the visual servoing skew vector
425 v1 = task.computeControlLaw();
426
427 err = task.error;
428
430 if (iter == 0) {
431 err_0 = 0;
432 err_1 = 0;
433 dedt_mes = 0;
434 dedt_filt = 0;
435 } else {
436 err_1 = err_0;
437 err_0 = err;
438
439 dedt_mes = (err_0 - err_1) / (Tv_1)-task.J1 * vm_0;
440 }
442 if (iter <= 1) {
443 dedt_mes = 0;
444 }
445
446 //----------------------------------------------------------------------
447 //----------------------- Kalman Filter Equations ----------------------
448 //----------------------------------------------------------------------
449 // Kalman filtering
450 switch (opt_kalman) {
451 case K_NONE:
452 dedt_filt = 0;
453 break;
454 case K_VELOCITY:
455 case K_ACCELERATION:
456 kalman.filter(dedt_mes);
457 for (unsigned int i = 0; i < nsignal; i++) {
458 dedt_filt[i] = kalman.Xest[i * state_size];
459 }
460 break;
461 }
462
465 v2 = -J1p * dedt_filt;
466 // std::cout << "task J1p: " << J1p.t() << std::endl ;
467 // std::cout << "dedt_filt: " << dedt_filt.t() << std::endl ;
468
469 v = v1 + v2;
470
471 // Display the current and desired feature points in the image display
472 vpServoDisplay::display(task, cam, I);
473
474 // std::cout << "v2 : " << v2.t() << std::endl ;
475 // std::cout << "v1 : " << v1.t() << std::endl ;
476
477 // std::cout << "v : " << v.t();
478
479 // Apply the camera velocities to the robot
481
482 // Save loop time
483 flog << Tv_0 << " ";
484
485 // Save velocities applied to the robot in the log file
486 // v[0], v[1], v[2] correspond to camera translation velocities in m/s
487 // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
488 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
489
490 // Save feature error (s-s*) for the feature point. For this feature
491 // point, we have 2 errors (along x and y axis). This error is
492 // expressed in meters in the camera frame
493 flog << task.error[0] << " " << task.error[1] << " ";
494
495 // Save feature error (s-s*) in pixels in the image.
496 flog << cog.get_u() - cam.get_u0() << " " << cog.get_v() - cam.get_v0() << " ";
497
498 // Save de/dt
499 flog << dedt_mes[0] << " " << dedt_mes[1] << " ";
500
501 // Save de/dt filtered
502 flog << dedt_filt[0] << " " << dedt_filt[1] << " ";
503
504 flog << std::endl;
505
506 // Flush the display
508
509 iter++;
510 }
511
512 flog.close(); // Close the log file
513
514 // Display task information
515 task.print();
516
517 return EXIT_SUCCESS;
518 } catch (const vpException &e) {
519 std::cout << "Catch a ViSP exception: " << e << std::endl;
520 return EXIT_FAILURE;
521 }
522}
523
524#else
525int main()
526{
527 std::cout << "You do not have an afma4 robot connected to your computer..." << std::endl;
528 return EXIT_SUCCESS;
529}
530#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Adaptive gain computation.
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
void initFromConstant(double c)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
static const vpColor blue
Definition vpColor.h:217
static const vpColor green
Definition vpColor.h:214
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
static void display(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition vpDot2.h:124
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition vpDot2.cpp:441
void setGraphics(bool activate)
Definition vpDot2.h:311
vpImagePoint getCog() const
Definition vpDot2.h:177
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition vpDot2.cpp:252
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double get_u() const
double get_v() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static bool checkDirectory(const std::string &dirname)
static std::string getUserName()
static void makeDirectory(const std::string &dirname)
vpColVector Xest
unsigned int getStateSize()
This class provides an implementation of some specific linear Kalman filters.
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Control of Irisa's cylindrical robot named Afma4.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
@ 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
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)
vpMatrix J1
Task Jacobian .
Definition vpServo.h:552
@ 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 error
Definition vpServo.h:550
vpColVector computeControlLaw()
Definition vpServo.cpp:930
vpMatrix getTaskJacobianPseudoInverse() const
Definition vpServo.cpp:1785
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
VISP_EXPORT double measureTimeMs()