Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoViper850Point2DCamVelocityKalman.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 camera frame
35 *
36*****************************************************************************/
37
49#include <visp3/core/vpConfig.h>
50#include <visp3/core/vpDebug.h> // Debug trace
51
52#include <fstream>
53#include <iostream>
54#include <sstream>
55#include <stdio.h>
56#include <stdlib.h>
57
58#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
59
60#include <visp3/blob/vpDot2.h>
61#include <visp3/core/vpDisplay.h>
62#include <visp3/core/vpException.h>
63#include <visp3/core/vpHomogeneousMatrix.h>
64#include <visp3/core/vpImage.h>
65#include <visp3/core/vpIoTools.h>
66#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
67#include <visp3/core/vpMath.h>
68#include <visp3/core/vpPoint.h>
69#include <visp3/gui/vpDisplayGTK.h>
70#include <visp3/gui/vpDisplayOpenCV.h>
71#include <visp3/gui/vpDisplayX.h>
72#include <visp3/io/vpImageIo.h>
73#include <visp3/robot/vpRobotViper850.h>
74#include <visp3/sensor/vp1394TwoGrabber.h>
75#include <visp3/visual_features/vpFeatureBuilder.h>
76#include <visp3/visual_features/vpFeaturePoint.h>
77#include <visp3/vs/vpAdaptiveGain.h>
78#include <visp3/vs/vpServo.h>
79#include <visp3/vs/vpServoDisplay.h>
80
81int main()
82{
83 // Log file creation in /tmp/$USERNAME/log.dat
84 // This file contains by line:
85 // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
86 // - the 6 mesured joint velocities (m/s, rad/s)
87 // - the 6 mesured joint positions (m, rad)
88 // - the 2 values of s - s*
89 std::string username;
90 // Get the user login name
91 vpIoTools::getUserName(username);
92
93 // Create a log filename to save velocities...
94 std::string logdirname;
95 logdirname = "/tmp/" + username;
96
97 // Test if the output path exist. If no try to create it
98 if (vpIoTools::checkDirectory(logdirname) == false) {
99 try {
100 // Create the dirname
101 vpIoTools::makeDirectory(logdirname);
102 } catch (...) {
103 std::cerr << std::endl << "ERROR:" << std::endl;
104 std::cerr << " Cannot create " << logdirname << std::endl;
105 return EXIT_FAILURE;
106 }
107 }
108 std::string logfilename;
109 logfilename = logdirname + "/log.dat";
110
111 // Open the log file name
112 std::ofstream flog(logfilename.c_str());
113
114 vpServo task;
115
116 try {
117 // Initialize linear Kalman filter
119
120 // Initialize the kalman filter
121 unsigned int nsignal = 2; // The two values of dedt
122 double rho = 0.3;
123 vpColVector sigma_state;
124 vpColVector sigma_measure(nsignal);
125 unsigned int state_size = 0; // Kalman state vector size
126
128 state_size = kalman.getStateSize();
129 sigma_state.resize(state_size * nsignal);
130 sigma_state = 0.00001; // Same state variance for all signals
131 sigma_measure = 0.05; // Same measure variance for all the signals
132 double dummy = 0; // non used parameter dt for the velocity state model
133 kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
134
135 // Initialize the robot
136 vpRobotViper850 robot;
137
139
140 bool reset = false;
141 vp1394TwoGrabber g(reset);
142
143#if 1
145 g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
146#else
149#endif
150 g.open(I);
151
152 double Tloop = 1. / 80.f;
153
155 g.getFramerate(fps);
156 switch (fps) {
158 Tloop = 1.f / 15.f;
159 break;
161 Tloop = 1.f / 30.f;
162 break;
164 Tloop = 1.f / 60.f;
165 break;
167 Tloop = 1.f / 120.f;
168 break;
169 default:
170 break;
171 }
172
173#ifdef VISP_HAVE_X11
174 vpDisplayX display(I, (int)(100 + I.getWidth() + 30), 200, "Current image");
175#elif defined(HAVE_OPENCV_HIGHGUI)
176 vpDisplayOpenCV display(I, (int)(100 + I.getWidth() + 30), 200, "Current image");
177#elif defined(VISP_HAVE_GTK)
178 vpDisplayGTK display(I, (int)(100 + I.getWidth() + 30), 200, "Current image");
179#endif
180
183
184 vpDot2 dot;
185 vpImagePoint cog;
186
187 dot.setGraphics(true);
188
189 for (int i = 0; i < 10; i++)
190 g.acquire(I);
191
192 std::cout << "Click on a dot..." << std::endl;
193 dot.initTracking(I);
194
195 cog = dot.getCog();
198
200 // Update camera parameters
201 robot.getCameraParameters(cam, I);
202
203 // sets the current position of the visual feature
205 // retrieve x,y and Z of the vpPoint structure
206 vpFeatureBuilder::create(p, cam, dot);
207
208 // sets the desired position of the visual feature
210 pd.buildFrom(0, 0, 1);
211
212 // define the task
213 // - we want an eye-in-hand control law
214 // - robot is controlled in the camera frame
217
218 // - we want to see a point on a point
219 task.addFeature(p, pd);
220
221 // - set the constant gain
222 vpAdaptiveGain lambda;
223 lambda.initStandard(4, 0.2, 30);
224 task.setLambda(lambda);
225
226 // Display task information
227 task.print();
228
229 // Now the robot will be controlled in velocity
231
232 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
233 vpColVector v, v1, v2;
234 int iter = 0;
235 vpColVector vm(6);
236 double t_0, t_1, Tv;
237 vpColVector err(2), err_1(2);
238 vpColVector dedt_filt(2), dedt_mes(2);
239 dc1394video_frame_t *frame = NULL;
240
241 t_1 = vpTime::measureTimeMs();
242
243 for (;;) {
244 try {
245 t_0 = vpTime::measureTimeMs(); // t_0: current time
246
247 // Update loop time in second
248 Tv = (double)(t_0 - t_1) / 1000.0;
249
250 // Update time for next iteration
251 t_1 = t_0;
252
254
255 // Acquire a new image from the camera
256 frame = g.dequeue(I);
257
258 // Display this image
260
261 // Achieve the tracking of the dot in the image
262 dot.track(I);
263
264 // Get the dot cog
265 cog = dot.getCog();
266
267 // Display a green cross at the center of gravity position in the
268 // image
270
271 // Update the point feature from the dot location
272 vpFeatureBuilder::create(p, cam, dot);
273
274 // Compute the visual servoing skew vector
275 v1 = task.computeControlLaw();
276
277 // Get the error ||s-s*||
278 err = task.getError();
279
281 if (iter == 0) {
282 err_1 = 0;
283 dedt_mes = 0;
284 } else {
285 vpMatrix J1 = task.getTaskJacobian();
286 dedt_mes = (err - err_1) / (Tv)-J1 * vm;
287 err_1 = err;
288 }
289
290 // Filter de/dt
291 if (iter < 2)
292 dedt_mes = 0;
293 kalman.filter(dedt_mes);
294 // Get the filtered values
295 for (unsigned int i = 0; i < nsignal; i++) {
296 dedt_filt[i] = kalman.Xest[i * state_size];
297 }
298 if (iter < 2)
299 dedt_filt = 0;
300
302 v2 = -J1p * dedt_filt;
303
304 // Update the robot camera velocity
305 v = v1 + v2;
306
307 // Display the current and desired feature points in the image display
308 vpServoDisplay::display(task, cam, I);
309
310 // Apply the computed camera velocities to the robot
312
313 iter++;
314 // Synchronize the loop with the image frame rate
315 vpTime::wait(t_0, 1000. * Tloop);
316 // Release the ring buffer used for the last image to start a new acq
317 g.enqueue(frame);
318 } catch (...) {
319 std::cout << "Tracking failed... Stop the robot." << std::endl;
320 v = 0;
321 // Stop robot
323 return EXIT_FAILURE;
324 }
325
326 // Save velocities applied to the robot in the log file
327 // v[0], v[1], v[2] correspond to camera translation velocities in m/s
328 // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
329 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
330
331 // Get the measured joint velocities of the robot
332 vpColVector qvel;
334 // Save measured joint velocities of the robot in the log file:
335 // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
336 // velocities in m/s
337 // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
338 // velocities in rad/s
339 flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
340
341 // Get the measured joint positions of the robot
342 vpColVector q;
343 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
344 // Save measured joint positions of the robot in the log file
345 // - q[0], q[1], q[2] correspond to measured joint translation
346 // positions in m
347 // - q[3], q[4], q[5] correspond to measured joint rotation
348 // positions in rad
349 flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
350
351 // Save feature error (s-s*) for the feature point. For this feature
352 // point, we have 2 errors (along x and y axis). This error is
353 // expressed in meters in the camera frame
354 flog << (task.getError()).t() << std::endl; // s-s* for point
355
356 // Flush the display
358 }
359
360 flog.close(); // Close the log file
361
362 // Display task information
363 task.print();
364
365 return EXIT_SUCCESS;
366 } catch (const vpException &e) {
367 flog.close(); // Close the log file
368 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
369 return EXIT_FAILURE;
370 }
371}
372
373#else
374int main()
375{
376 std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
377 return EXIT_SUCCESS;
378}
379#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)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
vpRowVector t() const
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
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
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
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ 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)
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
vpMatrix getTaskJacobian() const
Definition vpServo.cpp:1765
vpColVector getError() const
Definition vpServo.h:276
@ PSEUDO_INVERSE
Definition vpServo.h:199
vpColVector computeControlLaw()
Definition vpServo.cpp:930
vpMatrix getTaskJacobianPseudoInverse() const
Definition vpServo.cpp:1785
@ DESIRED
Definition vpServo.h:183
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)