1 /****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 articular frame
35 *
36 * Authors:
37 * Fabien Spindler
38 *
39 *****************************************************************************/
40 /*!
41 \example servoViper850FourPoints2DArtVelocityLs_des.cpp
42
43 \brief Example of eye-in-hand control law. We control here a real robot, the
44 Viper S850 robot (arm with 6 degrees of freedom). The velocities resulting
45 from visual servo are here joint velocities. Visual features are the image
46 coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by
47 10cm square.
48
49 */
50
51 #include <visp3/core/vpConfig.h>
52 #include <visp3/core/vpDebug.h> // Debug trace
53
54 #include <fstream>
55 #include <iostream>
56 #include <sstream>
57 #include <stdio.h>
58 #include <stdlib.h>
59 #if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
60
61 #include <visp3/blob/vpDot2.h>
62 #include <visp3/core/vpDisplay.h>
63 #include <visp3/core/vpHomogeneousMatrix.h>
64 #include <visp3/core/vpImage.h>
65 #include <visp3/core/vpIoTools.h>
66 #include <visp3/core/vpMath.h>
67 #include <visp3/core/vpPoint.h>
68 #include <visp3/gui/vpDisplayGTK.h>
69 #include <visp3/gui/vpDisplayOpenCV.h>
70 #include <visp3/gui/vpDisplayX.h>
71 #include <visp3/robot/vpRobotViper850.h>
72 #include <visp3/sensor/vp1394TwoGrabber.h>
73 #include <visp3/vision/vpPose.h>
74 #include <visp3/visual_features/vpFeatureBuilder.h>
75 #include <visp3/visual_features/vpFeaturePoint.h>
76 #include <visp3/vs/vpServo.h>
77 #include <visp3/vs/vpServoDisplay.h>
78
main()79 int main()
80 {
81 // Log file creation in /tmp/$USERNAME/log.dat
82 // This file contains by line:
83 // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
84 // - the 6 mesured joint velocities (m/s, rad/s)
85 // - the 6 mesured joint positions (m, rad)
86 // - the 8 values of s - s*
87 std::string username;
88 // Get the user login name
89 vpIoTools::getUserName(username);
90
91 // Create a log filename to save velocities...
92 std::string logdirname;
93 logdirname = "/tmp/" + username;
94
95 // Test if the output path exist. If no try to create it
96 if (vpIoTools::checkDirectory(logdirname) == false) {
97 try {
98 // Create the dirname
99 vpIoTools::makeDirectory(logdirname);
100 } catch (...) {
101 std::cerr << std::endl << "ERROR:" << std::endl;
102 std::cerr << " Cannot create " << logdirname << std::endl;
103 return (-1);
104 }
105 }
106 std::string logfilename;
107 logfilename = logdirname + "/log.dat";
108
109 // Open the log file name
110 std::ofstream flog(logfilename.c_str());
111
112 try {
113 // Define the square CAD model
114 // Square dimention
115 //#define L 0.075
116 #define L 0.05
117 // Distance between the camera and the square at the desired
118 // position after visual servoing convergence
119 #define D 0.5
120
121 vpRobotViper850 robot;
122 // Load the end-effector to camera frame transformation obtained
123 // using a camera intrinsic model with distortion
124 vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion;
125 robot.init(vpRobotViper850::TOOL_PTGREY_FLEA2_CAMERA, projModel);
126
127 vpServo task;
128
129 vpImage<unsigned char> I;
130 int i;
131
132 bool reset = false;
133 vp1394TwoGrabber g(reset);
134 g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
135 g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
136 g.open(I);
137
138 g.acquire(I);
139
140 #ifdef VISP_HAVE_X11
141 vpDisplayX display(I, 100, 100, "Current image");
142 #elif defined(VISP_HAVE_OPENCV)
143 vpDisplayOpenCV display(I, 100, 100, "Current image");
144 #elif defined(VISP_HAVE_GTK)
145 vpDisplayGTK display(I, 100, 100, "Current image");
146 #endif
147
148 vpDisplay::display(I);
149 vpDisplay::flush(I);
150
151 std::cout << std::endl;
152 std::cout << "-------------------------------------------------------" << std::endl;
153 std::cout << " Test program for vpServo " << std::endl;
154 std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl;
155 std::cout << " Use of the Afma6 robot " << std::endl;
156 std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl;
157 std::cout << "-------------------------------------------------------" << std::endl;
158 std::cout << std::endl;
159
160 vpDot dot[4];
161 vpImagePoint cog;
162
163 std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl;
164
165 for (i = 0; i < 4; i++) {
166 dot[i].setGraphics(true);
167 dot[i].initTracking(I);
168 cog = dot[i].getCog();
169 vpDisplay::displayCross(I, cog, 10, vpColor::blue);
170 vpDisplay::flush(I);
171 }
172
173 vpCameraParameters cam;
174
175 // Update camera parameters
176 robot.getCameraParameters(cam, I);
177
178 cam.printParameters();
179
180 // Sets the current position of the visual feature
181 vpFeaturePoint p[4];
182 for (i = 0; i < 4; i++)
183 vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y and Z of the vpPoint structure
184
185 // sets the desired position of the visual feature
186 vpFeaturePoint pd[4];
187
188 pd[0].buildFrom(-L, -L, D);
189 pd[1].buildFrom(L, -L, D);
190 pd[2].buildFrom(L, L, D);
191 pd[3].buildFrom(-L, L, D);
192
193 // We want to see a point on a point
194 std::cout << std::endl;
195 for (i = 0; i < 4; i++)
196 task.addFeature(p[i], pd[i]);
197
198 // Set the proportional gain
199 task.setLambda(0.4);
200
201 // Display task information
202 task.print();
203
204 // Define the task
205 // - we want an eye-in-hand control law
206 // - articular velocity are computed
207 task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
208 task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
209 task.print();
210
211 vpVelocityTwistMatrix cVe;
212 robot.get_cVe(cVe);
213 task.set_cVe(cVe);
214 task.print();
215
216 // Set the Jacobian (expressed in the end-effector frame)
217 vpMatrix eJe;
218 robot.get_eJe(eJe);
219 task.set_eJe(eJe);
220 task.print();
221
222 // Initialise the velocity control of the robot
223 robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
224
225 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
226 for (;;) {
227 // Acquire a new image from the camera
228 g.acquire(I);
229
230 // Display this image
231 vpDisplay::display(I);
232
233 try {
234 // For each point...
235 for (i = 0; i < 4; i++) {
236 // Achieve the tracking of the dot in the image
237 dot[i].track(I);
238 // Display a green cross at the center of gravity position in the
239 // image
240 cog = dot[i].getCog();
241 vpDisplay::displayCross(I, cog, 10, vpColor::green);
242 }
243 } catch (...) {
244 flog.close(); // Close the log file
245 vpTRACE("Error detected while tracking visual features");
246 robot.stopMotion();
247 exit(1);
248 }
249
250 // Update the point feature from the dot location
251 for (i = 0; i < 4; i++)
252 vpFeatureBuilder::create(p[i], cam, dot[i]);
253
254 // Get the jacobian of the robot
255 robot.get_eJe(eJe);
256 // Update this jacobian in the task structure. It will be used to
257 // compute the velocity skew (as an articular velocity) qdot = -lambda *
258 // L^+ * cVe * eJe * (s-s*)
259 task.set_eJe(eJe);
260
261 vpColVector v;
262 // Compute the visual servoing skew vector
263 v = task.computeControlLaw();
264
265 // Display the current and desired feature points in the image display
266 vpServoDisplay::display(task, cam, I);
267
268 // Apply the computed joint velocities to the robot
269 robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
270
271 // Save velocities applied to the robot in the log file
272 // v[0], v[1], v[2] correspond to joint translation velocities in m/s
273 // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s
274 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
275
276 // Get the measured joint velocities of the robot
277 vpColVector qvel;
278 robot.getVelocity(vpRobot::ARTICULAR_FRAME, qvel);
279 // Save measured joint velocities of the robot in the log file:
280 // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
281 // velocities in m/s
282 // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
283 // velocities in rad/s
284 flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
285
286 // Get the measured joint positions of the robot
287 vpColVector q;
288 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
289 // Save measured joint positions of the robot in the log file
290 // - q[0], q[1], q[2] correspond to measured joint translation
291 // positions in m
292 // - q[3], q[4], q[5] correspond to measured joint rotation
293 // positions in rad
294 flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
295
296 // Save feature error (s-s*) for the 4 feature points. For each feature
297 // point, we have 2 errors (along x and y axis). This error is
298 // expressed in meters in the camera frame
299 flog << (task.getError()).t() << std::endl;
300
301 // Flush the display
302 vpDisplay::flush(I);
303
304 // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<
305 // std::endl;
306 }
307
308 std::cout << "Display task information: " << std::endl;
309 task.print();
310 flog.close(); // Close the log file
311 return EXIT_SUCCESS;
312 }
313 catch (const vpException &e) {
314 flog.close(); // Close the log file
315 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
316 return EXIT_FAILURE;
317 }
318 }
319
320 #else
main()321 int main()
322 {
323 std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
324 return EXIT_SUCCESS;
325 }
326 #endif
327