ViSP  3.0.0
servoViper850Point2DCamVelocityKalman.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * tests the control law
32  * eye-in-hand control
33  * velocity computed in camera frame
34  *
35  * Authors:
36  * Eric Marchand
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 
41 
53 #include <visp3/core/vpConfig.h>
54 #include <visp3/core/vpDebug.h> // Debug trace
55 
56 #include <stdlib.h>
57 #include <stdio.h>
58 #include <iostream>
59 #include <fstream>
60 #include <sstream>
61 
62 #if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
63 
64 #include <visp3/sensor/vp1394TwoGrabber.h>
65 #include <visp3/core/vpImage.h>
66 #include <visp3/core/vpMath.h>
67 #include <visp3/core/vpHomogeneousMatrix.h>
68 #include <visp3/visual_features/vpFeaturePoint.h>
69 #include <visp3/core/vpPoint.h>
70 #include <visp3/vs/vpServo.h>
71 #include <visp3/visual_features/vpFeatureBuilder.h>
72 #include <visp3/robot/vpRobotViper850.h>
73 #include <visp3/core/vpIoTools.h>
74 #include <visp3/core/vpException.h>
75 #include <visp3/vs/vpServoDisplay.h>
76 #include <visp3/io/vpImageIo.h>
77 #include <visp3/blob/vpDot2.h>
78 #include <visp3/vs/vpAdaptiveGain.h>
79 #include <visp3/core/vpLinearKalmanFilterInstantiation.h>
80 #include <visp3/core/vpDisplay.h>
81 #include <visp3/gui/vpDisplayX.h>
82 #include <visp3/gui/vpDisplayOpenCV.h>
83 #include <visp3/gui/vpDisplayGTK.h>
84 
85 int
86 main()
87 {
88  // Log file creation in /tmp/$USERNAME/log.dat
89  // This file contains by line:
90  // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
91  // - the 6 mesured joint velocities (m/s, rad/s)
92  // - the 6 mesured joint positions (m, rad)
93  // - the 2 values of s - s*
94  std::string username;
95  // Get the user login name
96  vpIoTools::getUserName(username);
97 
98  // Create a log filename to save velocities...
99  std::string logdirname;
100  logdirname ="/tmp/" + username;
101 
102  // Test if the output path exist. If no try to create it
103  if (vpIoTools::checkDirectory(logdirname) == false) {
104  try {
105  // Create the dirname
106  vpIoTools::makeDirectory(logdirname);
107  }
108  catch (...) {
109  std::cerr << std::endl
110  << "ERROR:" << std::endl;
111  std::cerr << " Cannot create " << logdirname << std::endl;
112  exit(-1);
113  }
114  }
115  std::string logfilename;
116  logfilename = logdirname + "/log.dat";
117 
118  // Open the log file name
119  std::ofstream flog(logfilename.c_str());
120 
121  vpServo task ;
122 
123  try {
124  // Initialize linear Kalman filter
126 
127  // Initialize the kalman filter
128  unsigned int nsignal = 2; // The two values of dedt
129  double rho = 0.3;
130  vpColVector sigma_state;
131  vpColVector sigma_measure(nsignal);
132  unsigned int state_size = 0; // Kalman state vector size
133 
135  state_size = kalman.getStateSize();
136  sigma_state.resize(state_size*nsignal);
137  sigma_state = 0.00001; // Same state variance for all signals
138  sigma_measure = 0.05; // Same measure variance for all the signals
139  double dummy = 0; // non used parameter dt for the velocity state model
140  kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
141 
142  // Initialize the robot
143  vpRobotViper850 robot ;
144 
145 
147 
148  bool reset = false;
149  vp1394TwoGrabber g(reset);
150 
151 #if 1
153  g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
154 #else
156  g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
157 #endif
158  g.open(I) ;
159 
160  double Tloop = 1./80.f;
161 
163  g.getFramerate(fps);
164  switch(fps) {
165  case vp1394TwoGrabber::vpFRAMERATE_15 : Tloop = 1.f/15.f; break;
166  case vp1394TwoGrabber::vpFRAMERATE_30 : Tloop = 1.f/30.f; break;
167  case vp1394TwoGrabber::vpFRAMERATE_60 : Tloop = 1.f/60.f; break;
168  case vp1394TwoGrabber::vpFRAMERATE_120: Tloop = 1.f/120.f; break;
169  default: break;
170  }
171 
172 #ifdef VISP_HAVE_X11
173  vpDisplayX display(I, (int)(100+I.getWidth()+30), 200, "Current image") ;
174 #elif defined(VISP_HAVE_OPENCV)
175  vpDisplayOpenCV display(I, (int)(100+I.getWidth()+30), 200, "Current image") ;
176 #elif defined(VISP_HAVE_GTK)
177  vpDisplayGTK display(I, (int)(100+I.getWidth()+30), 200, "Current image") ;
178 #endif
179 
180  vpDisplay::display(I) ;
181  vpDisplay::flush(I) ;
182 
183  vpDot2 dot ;
184  vpImagePoint cog;
185 
186  dot.setGraphics(true);
187 
188  for (int i=0; i< 10; i++)
189  g.acquire(I) ;
190 
191  std::cout << "Click on a dot..." << std::endl;
192  dot.initTracking(I) ;
193 
194  cog = dot.getCog();
196  vpDisplay::flush(I);
197 
198  vpCameraParameters cam ;
199  // Update camera parameters
200  robot.getCameraParameters (cam, I);
201 
202  // sets the current position of the visual feature
203  vpFeaturePoint p ;
204  // retrieve x,y and Z of the vpPoint structure
205  vpFeatureBuilder::create(p,cam, dot);
206 
207  // sets the desired position of the visual feature
208  vpFeaturePoint pd ;
209  pd.buildFrom(0,0,1) ;
210 
211  // define the task
212  // - we want an eye-in-hand control law
213  // - robot is controlled in the camera frame
214  task.setServo(vpServo::EYEINHAND_CAMERA) ;
215  task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
216 
217  // - we want to see a point on a point
218  task.addFeature(p,pd) ;
219 
220  // - set the constant gain
221  vpAdaptiveGain lambda;
222  lambda.initStandard(4, 0.2, 30);
223  task.setLambda(lambda) ;
224 
225  // Display task information
226  task.print() ;
227 
228  // Now the robot will be controlled in velocity
230 
231  std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
232  vpColVector v, v1, v2 ;
233  int iter = 0;
234  vpColVector vm(6);
235  double t_0, t_1, Tv;
236  vpColVector err(2), err_1(2);
237  vpColVector dedt_filt(2), dedt_mes(2);
238  dc1394video_frame_t *frame = NULL;
239 
240  t_1 = vpTime::measureTimeMs();
241 
242  for ( ; ; ) {
243  try {
244  t_0 = vpTime::measureTimeMs(); // t_0: current time
245 
246  // Update loop time in second
247  Tv = (double)(t_0 - t_1) / 1000.0;
248 
249  // Update time for next iteration
250  t_1 = t_0;
251 
253 
254  // Acquire a new image from the camera
255  frame = g.dequeue(I);
256 
257  // Display this image
258  vpDisplay::display(I) ;
259 
260  // Achieve the tracking of the dot in the image
261  dot.track(I) ;
262 
263  // Get the dot cog
264  cog = dot.getCog();
265 
266  // Display a green cross at the center of gravity position in the image
268 
269  // Update the point feature from the dot location
270  vpFeatureBuilder::create(p, cam, dot);
271 
272  // Compute the visual servoing skew vector
273  v1 = task.computeControlLaw() ;
274 
275  // Get the error ||s-s*||
276  err = task.getError();
277 
279  if (iter==0){
280  err_1 = 0;
281  dedt_mes = 0;
282  }
283  else{
284  vpMatrix J1 = task.getTaskJacobian();
285  dedt_mes = (err - err_1)/(Tv) - J1 *vm;
286  err_1 = err;
287  }
288 
289  // Filter de/dt
290  if (iter < 2)
291  dedt_mes = 0;
292  kalman.filter(dedt_mes);
293  // Get the filtered values
294  for (unsigned int i=0; i < nsignal; i++) {
295  dedt_filt[i] = kalman.Xest[i*state_size];
296  }
297  if (iter < 2)
298  dedt_filt = 0;
299 
300  vpMatrix J1p = task.getTaskJacobianPseudoInverse();
301  v2 = - J1p*dedt_filt;
302 
303  // Update the robot camera velocity
304  v = v1 + v2;
305 
306  // Display the current and desired feature points in the image display
307  vpServoDisplay::display(task, cam, I) ;
308 
309  // Apply the computed camera velocities to the robot
311 
312  iter ++;
313  // Synchronize the loop with the image frame rate
314  vpTime::wait(t_0, 1000.*Tloop);
315  // Release the ring buffer used for the last image to start a new acq
316  g.enqueue(frame);
317  }
318  catch(...) {
319  std::cout << "Tracking failed... Stop the robot." << std::endl;
320  v = 0;
321  // Stop robot
323  // Kill the task
324  task.kill();
325  return 0;
326  }
327 
328  // Save velocities applied to the robot in the log file
329  // v[0], v[1], v[2] correspond to camera translation velocities in m/s
330  // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
331  flog << v[0] << " " << v[1] << " " << v[2] << " "
332  << v[3] << " " << v[4] << " " << v[5] << " ";
333 
334  // Get the measured joint velocities of the robot
335  vpColVector qvel;
337  // Save measured joint velocities of the robot in the log file:
338  // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
339  // velocities in m/s
340  // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
341  // velocities in rad/s
342  flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " "
343  << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
344 
345  // Get the measured joint positions of the robot
346  vpColVector q;
348  // Save measured joint positions of the robot in the log file
349  // - q[0], q[1], q[2] correspond to measured joint translation
350  // positions in m
351  // - q[3], q[4], q[5] correspond to measured joint rotation
352  // positions in rad
353  flog << q[0] << " " << q[1] << " " << q[2] << " "
354  << q[3] << " " << q[4] << " " << q[5] << " ";
355 
356  // Save feature error (s-s*) for the feature point. For this feature
357  // point, we have 2 errors (along x and y axis). This error is expressed
358  // in meters in the camera frame
359  flog << ( task.getError() ).t() << std::endl; // s-s* for point
360 
361  // Flush the display
362  vpDisplay::flush(I) ;
363  }
364 
365  flog.close() ; // Close the log file
366 
367  // Display task information
368  task.print() ;
369 
370  // Kill the task
371  task.kill();
372 
373  return 0;
374  }
375  catch (...)
376  {
377  flog.close() ; // Close the log file
378  // Kill the task
379  task.kill();
380  vpERROR_TRACE(" Test failed") ;
381  return 0;
382  }
383 }
384 
385 #else
386 int
387 main()
388 {
389  vpERROR_TRACE("You do not have a Viper robot or a firewire framegrabber connected to your computer...");
390 }
391 #endif
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
Adaptive gain computation.
unsigned int getStateSize()
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:335
unsigned int getWidth() const
Definition: vpImage.h:161
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpViper850.cpp:584
Control of Irisa&#39;s Viper S850 robot named Viper850.
#define vpERROR_TRACE
Definition: vpDebug.h:391
Define the X11 console to display images.
Definition: vpDisplayX.h:148
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
vpColVector Xest
static const vpColor green
Definition: vpColor.h:166
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)
Definition: vpDot2.cpp:461
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
vpImagePoint getCog() const
Definition: vpDot2.h:160
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:404
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Initialize the velocity controller.
Definition: vpRobot.h:68
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
The vpDisplayOpenCV allows to display image using the opencv library.
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Generic class defining intrinsic camera parameters.
static std::string getUserName()
Definition: vpIoTools.cpp:161
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:141
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void buildFrom(const double x, const double y, const double Z)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:262
This class provides an implementation of some specific linear Kalman filters.
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
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 setGraphics(const bool activate)
Definition: vpDot2.h:309
static const vpColor blue
Definition: vpColor.h:169
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217