Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6Cylinder2DCamVelocity.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
51#include <cmath> // std::fabs
52#include <limits> // numeric_limits
53#include <stdlib.h>
54#include <visp3/core/vpConfig.h>
55#include <visp3/core/vpDebug.h> // Debug trace
56#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
57
58#include <visp3/core/vpDisplay.h>
59#include <visp3/core/vpImage.h>
60#include <visp3/gui/vpDisplayGTK.h>
61#include <visp3/gui/vpDisplayOpenCV.h>
62#include <visp3/gui/vpDisplayX.h>
63#include <visp3/io/vpImageIo.h>
64#include <visp3/sensor/vp1394TwoGrabber.h>
65
66#include <visp3/core/vpCylinder.h>
67#include <visp3/core/vpHomogeneousMatrix.h>
68#include <visp3/core/vpMath.h>
69#include <visp3/me/vpMeLine.h>
70#include <visp3/visual_features/vpFeatureBuilder.h>
71#include <visp3/visual_features/vpFeatureLine.h>
72#include <visp3/vs/vpServo.h>
73
74#include <visp3/robot/vpRobotAfma6.h>
75
76 // Exception
77#include <visp3/core/vpException.h>
78#include <visp3/vs/vpServoDisplay.h>
79
80int main()
81{
82 try {
84
88 g.open(I);
89
90 g.acquire(I);
91
92#ifdef VISP_HAVE_X11
93 vpDisplayX display(I, 100, 100, "Current image");
94#elif defined(HAVE_OPENCV_HIGHGUI)
95 vpDisplayOpenCV display(I, 100, 100, "Current image");
96#elif defined(VISP_HAVE_GTK)
97 vpDisplayGTK display(I, 100, 100, "Current image");
98#endif
101
102 vpServo task;
103
104 std::cout << std::endl;
105 std::cout << "-------------------------------------------------------" << std::endl;
106 std::cout << " Test program for vpServo " << std::endl;
107 std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
108 std::cout << " Simulation " << std::endl;
109 std::cout << " task : servo a point " << std::endl;
110 std::cout << "-------------------------------------------------------" << std::endl;
111 std::cout << std::endl;
112
113 int i;
114 int nbline = 2;
115 vpMeLine line[nbline];
116
117 vpMe me;
118 me.setRange(10);
119 me.setPointsToTrack(100);
121 me.setThreshold(15);
122 me.setSampleStep(10);
123
124 // Initialize the tracking of the two edges of the cylinder
125 for (i = 0; i < nbline; i++) {
127 line[i].setMe(&me);
128
129 line[i].initTracking(I);
130 line[i].track(I);
131 }
132
133 vpRobotAfma6 robot;
134 // robot.move("zero.pos") ;
135
137 // Update camera parameters
138 robot.getCameraParameters(cam, I);
139
140 vpTRACE("sets the current position of the visual feature ");
141 vpFeatureLine p[nbline];
142 for (i = 0; i < nbline; i++)
143 vpFeatureBuilder::create(p[i], cam, line[i]);
144
145 vpTRACE("sets the desired position of the visual feature ");
146 vpCylinder cyld(0, 1, 0, 0, 0, 0, 0.04);
147
148 vpHomogeneousMatrix cMo(0, 0, 0.4, 0, 0, vpMath::rad(0));
149
150 cyld.project(cMo);
151
152 vpFeatureLine pd[nbline];
155
156 // Those lines are needed to keep the conventions define in vpMeLine
157 // (Those in vpLine are less restrictive) Another way to have the
158 // coordinates of the desired features is to learn them before executing
159 // the program.
160 pd[0].setRhoTheta(-fabs(pd[0].getRho()), 0);
161 pd[1].setRhoTheta(-fabs(pd[1].getRho()), M_PI);
162
163 vpTRACE("define the task");
164 vpTRACE("\t we want an eye-in-hand control law");
165 vpTRACE("\t robot is controlled in the camera frame");
168
169 vpTRACE("\t we want to see a two lines on two lines..");
170 std::cout << std::endl;
171 for (i = 0; i < nbline; i++)
172 task.addFeature(p[i], pd[i]);
173
174 vpTRACE("\t set the gain");
175 task.setLambda(0.2);
176
177 vpTRACE("Display task information ");
178 task.print();
179
181
182 unsigned int iter = 0;
183 vpTRACE("\t loop");
184 vpColVector v;
186 double lambda_av = 0.05;
187 double alpha = 0.2;
188 double beta = 3;
189 for (;;) {
190 std::cout << "---------------------------------------------" << iter << std::endl;
191
192 try {
193 g.acquire(I);
195
196 // Track the two edges and update the features
197 for (i = 0; i < nbline; i++) {
198 line[i].track(I);
199 line[i].display(I, vpColor::red);
200
201 vpFeatureBuilder::create(p[i], cam, line[i]);
202 vpTRACE("%f %f ", line[i].getRho(), line[i].getTheta());
203
204 p[i].display(cam, I, vpColor::red);
205 pd[i].display(cam, I, vpColor::green);
206 }
207
209
210 // Adaptative gain
211 double gain;
212 {
213 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
214 gain = lambda_av;
215 else {
216 gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
217 }
218 }
219 task.setLambda(gain);
220
221 v = task.computeControlLaw();
222
223 if (iter == 0)
226 }
227 catch (...) {
228 v = 0;
230 robot.stopMotion();
231 exit(1);
232 }
233
234 vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
235 iter++;
236 }
237
238 vpTRACE("Display task information ");
239 task.print();
240 return EXIT_SUCCESS;
241 }
242 catch (const vpException &e) {
243 std::cout << "Test failed with exception: " << e << std::endl;
244 return EXIT_FAILURE;
245 }
246}
247
248#else
249int main()
250{
251 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
252 return EXIT_SUCCESS;
253}
254
255#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
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