Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6FourPoints2DArtVelocity.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 articular frame
35 *
36*****************************************************************************/
56#include <fstream>
57#include <iostream>
58#include <sstream>
59#include <stdio.h>
60#include <stdlib.h>
61#include <visp3/core/vpConfig.h>
62#include <visp3/core/vpDebug.h> // Debug trace
63#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
64
65#include <visp3/core/vpDisplay.h>
66#include <visp3/core/vpImage.h>
67#include <visp3/core/vpImagePoint.h>
68#include <visp3/gui/vpDisplayGTK.h>
69#include <visp3/gui/vpDisplayOpenCV.h>
70#include <visp3/gui/vpDisplayX.h>
71#include <visp3/sensor/vp1394TwoGrabber.h>
72
73#include <visp3/core/vpHomogeneousMatrix.h>
74#include <visp3/core/vpIoTools.h>
75#include <visp3/core/vpMath.h>
76#include <visp3/core/vpPoint.h>
77#include <visp3/robot/vpRobotAfma6.h>
78#include <visp3/visual_features/vpFeatureBuilder.h>
79#include <visp3/visual_features/vpFeaturePoint.h>
80#include <visp3/vs/vpServo.h>
81
82// Exception
83#include <visp3/core/vpException.h>
84#include <visp3/vs/vpServoDisplay.h>
85
86#include <visp3/blob/vpDot.h>
87
88int main()
89{
90 // Log file creation in /tmp/$USERNAME/log.dat
91 // This file contains by line:
92 // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
93 // - the 6 mesured joint velocities (m/s, rad/s)
94 // - the 6 mesured joint positions (m, rad)
95 // - the 8 values of s - s*
96 std::string username;
97 // Get the user login name
98 vpIoTools::getUserName(username);
99
100 // Create a log filename to save velocities...
101 std::string logdirname;
102 logdirname = "/tmp/" + username;
103
104 // Test if the output path exist. If no try to create it
105 if (vpIoTools::checkDirectory(logdirname) == false) {
106 try {
107 // Create the dirname
108 vpIoTools::makeDirectory(logdirname);
109 } catch (...) {
110 std::cerr << std::endl << "ERROR:" << std::endl;
111 std::cerr << " Cannot create " << logdirname << std::endl;
112 return EXIT_FAILURE;
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 try {
122// Define the square CAD model
123// Square dimention
124#define L 0.075
125// Distance between the camera and the square at the desired
126// position after visual servoing convergence
127#define D 0.5
128
129 vpServo task;
130
132 int i;
133
137 g.open(I);
138
139 g.acquire(I);
140
141#ifdef VISP_HAVE_X11
142 vpDisplayX display(I, 100, 100, "Current image");
143#elif defined(HAVE_OPENCV_HIGHGUI)
144 vpDisplayOpenCV display(I, 100, 100, "Current image");
145#elif defined(VISP_HAVE_GTK)
146 vpDisplayGTK display(I, 100, 100, "Current image");
147#endif
148
151
152 std::cout << std::endl;
153 std::cout << "-------------------------------------------------------" << std::endl;
154 std::cout << " Test program for vpServo " << std::endl;
155 std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl;
156 std::cout << " Use of the Afma6 robot " << std::endl;
157 std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl;
158 std::cout << "-------------------------------------------------------" << std::endl;
159 std::cout << std::endl;
160
161 vpDot dot[4];
162 vpImagePoint cog;
163
164 std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl;
165
166 for (i = 0; i < 4; i++) {
167 dot[i].initTracking(I);
168 cog = dot[i].getCog();
171 }
172
173 vpRobotAfma6 robot;
174
176
177 // Update camera parameters
178 robot.getCameraParameters(cam, I);
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.2);
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
209 task.print();
210
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
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
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 // Get the dot cog
239 cog = dot[i].getCog();
240 // Display a green cross at the center of gravity position in the
241 // image
243 }
244 } catch (...) {
245 flog.close(); // Close the log file
246 vpTRACE("Error detected while tracking visual features");
247 robot.stopMotion();
248 exit(1);
249 }
250
251 // Update the point feature from the dot location
252 for (i = 0; i < 4; i++)
253 vpFeatureBuilder::create(p[i], cam, dot[i]);
254
255 // Get the jacobian of the robot
256 robot.get_eJe(eJe);
257 // Update this jacobian in the task structure. It will be used to
258 // compute the velocity skew (as an articular velocity) qdot = -lambda *
259 // L^+ * cVe * eJe * (s-s*)
260 task.set_eJe(eJe);
261
262 vpColVector v;
263 // Compute the visual servoing skew vector
264 v = task.computeControlLaw();
265
266 // Display the current and desired feature points in the image display
267 vpServoDisplay::display(task, cam, I);
268
269 // Apply the computed joint velocities to the robot
271
272 // Save velocities applied to the robot in the log file
273 // v[0], v[1], v[2] correspond to joint translation velocities in m/s
274 // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s
275 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
276
277 // Get the measured joint velocities of the robot
278 vpColVector qvel;
280 // Save measured joint velocities of the robot in the log file:
281 // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
282 // velocities in m/s
283 // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
284 // velocities in rad/s
285 flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
286
287 // Get the measured joint positions of the robot
288 vpColVector q;
289 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
290 // Save measured joint positions of the robot in the log file
291 // - q[0], q[1], q[2] correspond to measured joint translation
292 // positions in m
293 // - q[3], q[4], q[5] correspond to measured joint rotation
294 // positions in rad
295 flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
296
297 // Save feature error (s-s*) for the 4 feature points. For each feature
298 // point, we have 2 errors (along x and y axis). This error is
299 // expressed in meters in the camera frame
300 flog << (task.getError()).t() << std::endl;
301
302 // Flush the display
304 }
305
306 vpTRACE("Display task information ");
307 task.print();
308 flog.close(); // Close the log file
309 return EXIT_SUCCESS;
310 } catch (const vpException &e) {
311 flog.close(); // Close the log file
312 std::cout << "Test failed with exception: " << e << std::endl;
313 return EXIT_FAILURE;
314 }
315}
316
317#else
318int main()
319{
320 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
321 return EXIT_SUCCESS;
322}
323
324#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
void setVideoMode(vp1394TwoVideoModeType videomode)
void setFramerate(vp1394TwoFramerateType fps)
void open(vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
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 dot (connected pixels with same gray level) on a vpImage.
Definition vpDot.h:112
void initTracking(const vpImage< unsigned char > &I)
Definition vpDot.cpp:617
vpImagePoint getCog() const
Definition vpDot.h:243
void track(const vpImage< unsigned char > &I)
Definition vpDot.cpp:757
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 ...
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)
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Control of Irisa's gantry robot named Afma6.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
void get_eJe(vpMatrix &eJe)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ 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_L_cVe_eJe
Definition vpServo.h:155
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition vpServo.h:448
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 set_eJe(const vpMatrix &eJe_)
Definition vpServo.h:506
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
@ DESIRED
Definition vpServo.h:183
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
vpVelocityTwistMatrix get_cVe() const
Definition vpUnicycle.h:79
#define vpTRACE
Definition vpDebug.h:411