Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoViper850FourPointsKinect.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*****************************************************************************/
47#include <visp3/core/vpConfig.h>
48#include <visp3/core/vpDebug.h> // Debug trace
49
50#include <fstream>
51#include <iostream>
52#include <sstream>
53#include <stdio.h>
54#include <stdlib.h>
55
56#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES))
57
58#include <visp3/core/vpDisplay.h>
59#include <visp3/core/vpHomogeneousMatrix.h>
60#include <visp3/core/vpImage.h>
61#include <visp3/core/vpImageConvert.h>
62#include <visp3/core/vpIoTools.h>
63#include <visp3/core/vpMath.h>
64#include <visp3/core/vpPoint.h>
65#include <visp3/gui/vpDisplayGTK.h>
66#include <visp3/gui/vpDisplayOpenCV.h>
67#include <visp3/gui/vpDisplayX.h>
68#include <visp3/robot/vpRobotViper850.h>
69#include <visp3/sensor/vp1394TwoGrabber.h>
70#include <visp3/sensor/vpKinect.h>
71#include <visp3/vision/vpPose.h>
72#include <visp3/visual_features/vpFeatureBuilder.h>
73#include <visp3/visual_features/vpFeaturePoint.h>
74#include <visp3/vs/vpServo.h>
75
76// Exception
77#include <visp3/core/vpException.h>
78#include <visp3/vs/vpServoDisplay.h>
79
80#include <visp3/blob/vpDot2.h>
81#define L 0.05 // to deal with a 10cm by 10cm square
82
104void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, vpCameraParameters cam, vpHomogeneousMatrix &cMo, bool init)
105{
107 vpPose pose;
108 vpImagePoint cog;
109 for (int i = 0; i < ndot; i++) {
110
111 double x = 0, y = 0;
112 cog = dot[i].getCog();
114 y); // pixel to meter conversion
115 point[i].set_x(x); // projection perspective p
116 point[i].set_y(y);
117 pose.addPoint(point[i]);
118 }
119
120 if (init == true) {
122 } else { // init = false; use of the previous pose to initialise VIRTUAL_VS
124 }
125}
126
127int main()
128{
129 // Log file creation in /tmp/$USERNAME/log.dat
130 // This file contains by line:
131 // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
132 // - the 6 mesured joint velocities (m/s, rad/s)
133 // - the 6 mesured joint positions (m, rad)
134 // - the 8 values of s - s*
135 std::string username;
136 // Get the user login name
137 vpIoTools::getUserName(username);
138
139 // Create a log filename to save velocities...
140 std::string logdirname;
141 logdirname = "/tmp/" + username;
142
143 // Test if the output path exist. If no try to create it
144 if (vpIoTools::checkDirectory(logdirname) == false) {
145 try {
146 // Create the dirname
147 vpIoTools::makeDirectory(logdirname);
148 } catch (...) {
149 std::cerr << std::endl << "ERROR:" << std::endl;
150 std::cerr << " Cannot create " << logdirname << std::endl;
151 return EXIT_FAILURE;
152 }
153 }
154 std::string logfilename;
155 logfilename = logdirname + "/log.dat";
156
157 // Open the log file name
158 std::ofstream flog(logfilename.c_str());
159
160 try {
161 vpRobotViper850 robot;
162 // Load the end-effector to camera frame transformation obtained
163 // using a camera intrinsic model with distortion
166
167 vpServo task;
168
170 vpImage<vpRGBa> Irgb;
171 int i;
172
173#ifdef VISP_HAVE_LIBFREENECT_OLD
174 // This is the way to initialize Freenect with an old version of
175 // libfreenect packages under ubuntu lucid 10.04
176 Freenect::Freenect<vpKinect> freenect;
177 vpKinect &kinect = freenect.createDevice(0);
178#else
179 Freenect::Freenect freenect;
180 vpKinect &kinect = freenect.createDevice<vpKinect>(0);
181#endif
182
184 kinect.getRGB(Irgb);
186
187#ifdef VISP_HAVE_X11
188 vpDisplayX display(I, 100, 100, "Current image");
189#elif defined(HAVE_OPENCV_HIGHGUI)
190 vpDisplayOpenCV display(I, 100, 100, "Current image");
191#elif defined(VISP_HAVE_GTK)
192 vpDisplayGTK display(I, 100, 100, "Current image");
193#endif
194
197
198 std::cout << std::endl;
199 std::cout << "-------------------------------------------------------" << std::endl;
200 std::cout << " Test program for vpServo " << std::endl;
201 std::cout << " Eye-in-hand task control, velocity computed in the camera space" << std::endl;
202 std::cout << " Use of the Viper850 robot " << std::endl;
203 std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl;
204 std::cout << "-------------------------------------------------------" << std::endl;
205 std::cout << std::endl;
206
207 vpDot2 dot[4];
208 vpImagePoint cog;
209
210 std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl;
211
212 for (i = 0; i < 4; i++) {
213 dot[i].initTracking(I);
214 cog = dot[i].getCog();
217 }
218
219 // Get Kinect Camera Parameters
221 // kinect.getRGBCamParameters(cam);
222
223 robot.getCameraParameters(cam, I);
224
225 cam.printParameters();
226
227 // Sets the current position of the visual feature
228 vpFeaturePoint p[4];
229 for (i = 0; i < 4; i++)
230 vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure
231
232 // Set the position of the square target in a frame which origin is
233 // centered in the middle of the square
234 vpPoint point[4];
235 point[0].setWorldCoordinates(-L, -L, 0);
236 point[1].setWorldCoordinates(L, -L, 0);
237 point[2].setWorldCoordinates(L, L, 0);
238 point[3].setWorldCoordinates(-L, L, 0);
239
240 // Initialise a desired pose to compute s*, the desired 2D point features
242 vpTranslationVector cto(0, 0, 0.5); // tz = 0.5 meter
244 vpRotationMatrix cRo(cro); // Build the rotation matrix
245 cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
246
247 // Sets the desired position of the 2D visual feature
248 vpFeaturePoint pd[4];
249 // Compute the desired position of the features from the desired pose
250 for (int i = 0; i < 4; i++) {
251 vpColVector cP, p;
252 point[i].changeFrame(cMo, cP);
253 point[i].projection(cP, p);
254
255 pd[i].set_x(p[0]);
256 pd[i].set_y(p[1]);
257 pd[i].set_Z(cP[2]);
258 }
259
260 // We want to see a point on a point
261 for (i = 0; i < 4; i++)
262 task.addFeature(p[i], pd[i]);
263
264 // Set the proportional gain
265 task.setLambda(0.5);
266
267 // Display task information
268 task.print();
269
270 // Define the task
271 // - we want an eye-in-hand control law
272 // - articular velocity are computed
275 task.print();
276
277 // Initialise the velocity control of the robot
279
280 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
281 for (;;) {
282 // Acquire a new image from the kinect
283 kinect.getRGB(Irgb);
285
286 // Display this image
288
289 try {
290 // For each point...
291 for (i = 0; i < 4; i++) {
292 // Achieve the tracking of the dot in the image
293 dot[i].track(I);
294 // Display a green cross at the center of gravity position in the
295 // image
296 cog = dot[i].getCog();
298 }
299 } catch (...) {
300 flog.close(); // Close the log file
301 vpTRACE("Error detected while tracking visual features");
302 robot.stopMotion();
303 kinect.stop();
304 return EXIT_FAILURE;
305 }
306
307 // At first iteration, we initialise non linear pose estimation with a linear approach.
308 // For the other iterations, non linear pose estimation is initialized with the pose estimated at previous
309 // iteration of the loop
310 compute_pose(point, dot, 4, cam, cMo, init_pose_from_linear_method);
311 if (init_pose_from_linear_method) {
312 init_pose_from_linear_method = false;
313 }
314 for (i = 0; i < 4; i++) {
315 // Update the point feature from the dot location
316 vpFeatureBuilder::create(p[i], cam, dot[i]);
317 // Set the feature Z coordinate from the pose
318 vpColVector cP;
319 point[i].changeFrame(cMo, cP);
320
321 p[i].set_Z(cP[2]);
322 }
323
324 vpColVector v;
325 // Compute the visual servoing skew vector
326 v = task.computeControlLaw();
327
328 // Display the current and desired feature points in the image display
329 vpServoDisplay::display(task, cam, I);
330
331 // Apply the computed joint velocities to the robot
333
334 // Save velocities applied to the robot in the log file
335 // v[0], v[1], v[2] correspond to joint translation velocities in m/s
336 // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s
337 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
338
339 // Get the measured joint velocities of the robot
340 vpColVector qvel;
342 // Save measured joint velocities of the robot in the log file:
343 // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
344 // velocities in m/s
345 // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
346 // velocities in rad/s
347 flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
348
349 // Get the measured joint positions of the robot
350 vpColVector q;
351 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
352 // Save measured joint positions of the robot in the log file
353 // - q[0], q[1], q[2] correspond to measured joint translation
354 // positions in m
355 // - q[3], q[4], q[5] correspond to measured joint rotation
356 // positions in rad
357 flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
358
359 // Save feature error (s-s*) for the 4 feature points. For each feature
360 // point, we have 2 errors (along x and y axis). This error is
361 // expressed in meters in the camera frame
362 flog << (task.getError()).t() << std::endl;
363
364 // Flush the display
366
367 // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<
368 // std::endl;
369 }
370
371 kinect.stop();
372 std::cout << "Display task information: " << std::endl;
373 task.print();
374 flog.close(); // Close the log file
375 return EXIT_SUCCESS;
376 } catch (const vpException &e) {
377 flog.close(); // Close the log file
378 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
379 return EXIT_FAILURE;
380 }
381}
382
383#else
384int main()
385{
386 std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
387 return EXIT_SUCCESS;
388}
389#endif
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
vpRowVector t() const
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
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 set_y(double y)
void set_x(double x)
void set_Z(double Z)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
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
static bool checkDirectory(const std::string &dirname)
static std::string getUserName()
static void makeDirectory(const std::string &dirname)
Driver for the Kinect-1 device.
Definition vpKinect.h:110
void stop()
Definition vpKinect.cpp:112
void start(vpKinect::vpDMResolution res=DMAP_LOW_RES)
Definition vpKinect.cpp:73
@ DMAP_LOW_RES
Definition vpKinect.h:125
bool getRGB(vpImage< vpRGBa > &IRGB)
Definition vpKinect.cpp:224
static double rad(double deg)
Definition vpMath.h:116
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
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 set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
void projection(const vpColVector &_cP, vpColVector &_p) const
Definition vpPoint.cpp:219
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition vpPoint.cpp:236
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
@ VIRTUAL_VS
Definition vpPose.h:96
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
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
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
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
vpColVector getError() const
Definition vpServo.h:276
@ PSEUDO_INVERSE
Definition vpServo.h:199
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 consider the case of a translation vector.
@ TOOL_GENERIC_CAMERA
Definition vpViper850.h:129
#define vpTRACE
Definition vpDebug.h:411