Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6Cylinder2DCamVelocitySecondaryTask.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*****************************************************************************/
37
55#include <cmath> // std::fabs
56#include <limits> // numeric_limits
57#include <stdlib.h>
58#include <visp3/core/vpConfig.h>
59#include <visp3/core/vpDebug.h> // Debug trace
60#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
61
62#include <visp3/core/vpDisplay.h>
63#include <visp3/core/vpImage.h>
64#include <visp3/gui/vpDisplayGTK.h>
65#include <visp3/gui/vpDisplayOpenCV.h>
66#include <visp3/gui/vpDisplayX.h>
67#include <visp3/io/vpImageIo.h>
68#include <visp3/sensor/vp1394TwoGrabber.h>
69
70#include <visp3/core/vpCylinder.h>
71#include <visp3/core/vpHomogeneousMatrix.h>
72#include <visp3/core/vpMath.h>
73#include <visp3/me/vpMeLine.h>
74#include <visp3/visual_features/vpFeatureBuilder.h>
75#include <visp3/visual_features/vpFeatureLine.h>
76#include <visp3/vs/vpServo.h>
77
78#include <visp3/robot/vpRobotAfma6.h>
79
80// Exception
81#include <visp3/core/vpException.h>
82#include <visp3/vs/vpServoDisplay.h>
83
84int main()
85{
86 try {
88
92 g.open(I);
93
94 g.acquire(I);
95
96#ifdef VISP_HAVE_X11
97 vpDisplayX display(I, 100, 100, "Current image");
98#elif defined(HAVE_OPENCV_HIGHGUI)
99 vpDisplayOpenCV display(I, 100, 100, "Current image");
100#elif defined(VISP_HAVE_GTK)
101 vpDisplayGTK display(I, 100, 100, "Current image");
102#endif
103
106
107 vpServo task;
108
109 std::cout << std::endl;
110 std::cout << "-------------------------------------------------------" << std::endl;
111 std::cout << " Test program for vpServo " << std::endl;
112 std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
113 std::cout << " Simulation " << std::endl;
114 std::cout << " task : servo a point " << std::endl;
115 std::cout << "-------------------------------------------------------" << std::endl;
116 std::cout << std::endl;
117
118 int i;
119 int nbline = 2;
120 vpMeLine line[nbline];
121
122 vpMe me;
123 me.setRange(20);
124 me.setPointsToTrack(100);
126 me.setThreshold(15);
127 me.setSampleStep(10);
128
129 // Initialize the tracking of the two edges of the cylinder
130 for (i = 0; i < nbline; i++) {
132 line[i].setMe(&me);
133
134 line[i].initTracking(I);
135 line[i].track(I);
136 }
137
138 vpRobotAfma6 robot;
139 // robot.move("zero.pos") ;
140
142 // Update camera parameters
143 robot.getCameraParameters(cam, I);
144
145 vpTRACE("sets the current position of the visual feature ");
146 vpFeatureLine p[nbline];
147 for (i = 0; i < nbline; i++)
148 vpFeatureBuilder::create(p[i], cam, line[i]);
149
150 vpTRACE("sets the desired position of the visual feature ");
151 vpCylinder cyld(0, 1, 0, 0, 0, 0, 0.04);
152
153 vpHomogeneousMatrix cMo(0, 0, 0.5, 0, 0, vpMath::rad(0));
154
155 cyld.project(cMo);
156
157 vpFeatureLine pd[nbline];
160
161 // Those lines are needed to keep the conventions define in vpMeLine
162 // (Those in vpLine are less restrictive) Another way to have the
163 // coordinates of the desired features is to learn them before executing
164 // the program.
165 pd[0].setRhoTheta(-fabs(pd[0].getRho()), 0);
166 pd[1].setRhoTheta(-fabs(pd[1].getRho()), M_PI);
167
168 vpTRACE("define the task");
169 vpTRACE("\t we want an eye-in-hand control law");
170 vpTRACE("\t robot is controlled in the camera frame");
173
174 vpTRACE("\t we want to see a point on a point..");
175 std::cout << std::endl;
176 for (i = 0; i < nbline; i++)
177 task.addFeature(p[i], pd[i]);
178
179 vpTRACE("\t set the gain");
180 task.setLambda(0.3);
181
182 vpTRACE("Display task information ");
183 task.print();
184
186
187 unsigned int iter = 0;
188 vpTRACE("\t loop");
189 vpColVector v;
191 double lambda_av = 0.05;
192 double alpha = 0.02;
193 double beta = 3;
194 double erreur = 1;
195
196 // First loop to reach the convergence position
197 while (erreur > 0.00001) {
198 std::cout << "---------------------------------------------" << iter << std::endl;
199
200 try {
201 g.acquire(I);
203
204 // Track the two edges and update the features
205 for (i = 0; i < nbline; i++) {
206 line[i].track(I);
207 line[i].display(I, vpColor::red);
208
209 vpFeatureBuilder::create(p[i], cam, line[i]);
210
211 p[i].display(cam, I, vpColor::red);
212 pd[i].display(cam, I, vpColor::green);
213 }
214
216
217 // Adaptative gain
218 double gain;
219 {
220 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
221 gain = lambda_av;
222 else {
223 gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
224 }
225 }
226 task.setLambda(gain);
227
228 v = task.computeControlLaw();
229
230 if (iter == 0)
232 }
233 catch (...) {
234 v = 0;
236 robot.stopMotion();
237 exit(1);
238 }
239
241 erreur = (task.getError()).sumSquare();
242 vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
243 iter++;
244 }
245
246 /**********************************************************************************************/
247
248 // Second loop is to compute the control while taking into account the
249 // secondary task.
250 vpColVector e1(6);
251 e1 = 0;
252 vpColVector e2(6);
253 e2 = 0;
254 vpColVector proj_e1;
255 vpColVector proj_e2;
256 iter = 0;
257 double rapport = 0;
258 double vitesse = 0.02;
259 unsigned int tempo = 1200;
260
261 for (;;) {
262 std::cout << "---------------------------------------------" << iter << std::endl;
263
264 try {
265 g.acquire(I);
267
268 // Track the two edges and update the features
269 for (i = 0; i < nbline; i++) {
270 line[i].track(I);
271 line[i].display(I, vpColor::red);
272
273 vpFeatureBuilder::create(p[i], cam, line[i]);
274
275 p[i].display(cam, I, vpColor::red);
276 pd[i].display(cam, I, vpColor::green);
277 }
278
280
281 v = task.computeControlLaw();
282
283 // Compute the new control law corresponding to the secondary task
284 if (iter % tempo < 400 /*&& iter%tempo >= 0*/) {
285 e2 = 0;
286 e1[0] = fabs(vitesse);
287 proj_e1 = task.secondaryTask(e1);
288 rapport = vitesse / proj_e1[0];
289 proj_e1 *= rapport;
290 v += proj_e1;
291 if (iter == 199)
292 iter += 200; // This line is needed to make on ly an half turn
293 // during the first cycle
294 }
295
296 if (iter % tempo < 600 && iter % tempo >= 400) {
297 e1 = 0;
298 e2[1] = fabs(vitesse);
299 proj_e2 = task.secondaryTask(e2);
300 rapport = vitesse / proj_e2[1];
301 proj_e2 *= rapport;
302 v += proj_e2;
303 }
304
305 if (iter % tempo < 1000 && iter % tempo >= 600) {
306 e2 = 0;
307 e1[0] = -fabs(vitesse);
308 proj_e1 = task.secondaryTask(e1);
309 rapport = -vitesse / proj_e1[0];
310 proj_e1 *= rapport;
311 v += proj_e1;
312 }
313
314 if (iter % tempo < 1200 && iter % tempo >= 1000) {
315 e1 = 0;
316 e2[1] = -fabs(vitesse);
317 proj_e2 = task.secondaryTask(e2);
318 rapport = -vitesse / proj_e2[1];
319 proj_e2 *= rapport;
320 v += proj_e2;
321 }
322
324 }
325 catch (...) {
326 v = 0;
328 robot.stopMotion();
329 exit(1);
330 }
331
332 vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
333 iter++;
334 }
335
336 vpTRACE("Display task information ");
337 task.print();
338 return EXIT_SUCCESS;
339 }
340 catch (const vpException &e) {
341 std::cout << "Test failed with exception: " << e << std::endl;
342 return EXIT_FAILURE;
343 }
344}
345
346#else
347int main()
348{
349 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
350 return EXIT_SUCCESS;
351}
352
353#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 red
Definition vpColor.h:211
static const vpColor green
Definition vpColor.h:214
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition vpCylinder.h:98
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 bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
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 line visual feature which is composed by two parameters that are and ,...
void setRhoTheta(double rho, double theta)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double rad(double deg)
Definition vpMath.h:116
Class that tracks in an image a line moving edges.
Definition vpMeLine.h:148
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1)
Definition vpMeLine.cpp:181
void track(const vpImage< unsigned char > &I)
Definition vpMeLine.cpp:649
void initTracking(const vpImage< unsigned char > &I)
Definition vpMeLine.cpp:186
@ RANGE_RESULT
Definition vpMeSite.h:75
void setDisplay(vpMeSite::vpMeSiteDisplayType select)
void setMe(vpMe *p_me)
Definition vpMe.h:122
void setSampleStep(const double &s)
Definition vpMe.h:390
void setRange(const unsigned int &r)
Definition vpMe.h:383
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
Definition vpMe.h:445
void setPointsToTrack(const int &n)
Definition vpMe.h:376
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
Definition vpMe.h:132
void setThreshold(const double &t)
Definition vpMe.h:435
Control of Irisa's gantry robot named Afma6.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ 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
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
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition vpServo.cpp:1454
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
#define vpTRACE
Definition vpDebug.h:411