Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6TwoLines2DCamVelocity.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
58#include <cmath> // std::fabs
59#include <limits> // numeric_limits
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/gui/vpDisplayGTK.h>
68#include <visp3/gui/vpDisplayOpenCV.h>
69#include <visp3/gui/vpDisplayX.h>
70#include <visp3/io/vpImageIo.h>
71#include <visp3/sensor/vp1394TwoGrabber.h>
72
73#include <visp3/core/vpHomogeneousMatrix.h>
74#include <visp3/core/vpLine.h>
75#include <visp3/core/vpMath.h>
76#include <visp3/me/vpMeLine.h>
77#include <visp3/visual_features/vpFeatureBuilder.h>
78#include <visp3/visual_features/vpFeatureLine.h>
79#include <visp3/vs/vpServo.h>
80
81#include <visp3/robot/vpRobotAfma6.h>
82
83// Exception
84#include <visp3/core/vpException.h>
85#include <visp3/vs/vpServoDisplay.h>
86
87int main()
88{
89 try {
91
95 g.open(I);
96
97 g.acquire(I);
98
99#ifdef VISP_HAVE_X11
100 vpDisplayX display(I, 100, 100, "Current image");
101#elif defined(HAVE_OPENCV_HIGHGUI)
102 vpDisplayOpenCV display(I, 100, 100, "Current image");
103#elif defined(VISP_HAVE_GTK)
104 vpDisplayGTK display(I, 100, 100, "Current image");
105#endif
106
109
110 vpServo task;
111
112 std::cout << std::endl;
113 std::cout << "-------------------------------------------------------" << std::endl;
114 std::cout << " Test program for vpServo " << std::endl;
115 std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
116 std::cout << " Simulation " << std::endl;
117 std::cout << " task : servo a point " << std::endl;
118 std::cout << "-------------------------------------------------------" << std::endl;
119 std::cout << std::endl;
120
121 int i;
122 int nbline = 2;
123
124 vpMeLine line[nbline];
125
126 vpMe me;
127 me.setRange(10);
128 me.setPointsToTrack(100);
130 me.setThreshold(15);
131 me.setSampleStep(10);
132
133 // Initialize the tracking. Define the two lines to track
134 vpTRACE("The two lines to track must be parallels ");
135 // vpTRACE("The two lines to track must be perpendicular ") ;
136 for (i = 0; i < nbline; i++) {
138 line[i].setMe(&me);
139
140 line[i].initTracking(I);
141 line[i].track(I);
142 }
143
144 vpRobotAfma6 robot;
145 // robot.move("zero.pos") ;
146
148 // Update camera parameters
149 robot.getCameraParameters(cam, I);
150
151 vpTRACE("sets the current position of the visual feature ");
152 vpFeatureLine p[nbline];
153 for (i = 0; i < nbline; i++)
154 vpFeatureBuilder::create(p[i], cam, line[i]);
155
156 vpTRACE("sets the desired position of the visual feature ");
157 vpLine lined[2];
158 lined[0].setWorldCoordinates(1, 0, 0, -0.05, 0, 0, 1, 0);
159 lined[1].setWorldCoordinates(1, 0, 0, 0.05, 0, 0, 1, 0);
160
161 vpHomogeneousMatrix cMo(0, 0, 0.5, 0, 0, vpMath::rad(0));
162
163 lined[0].project(cMo);
164 lined[1].project(cMo);
165
166 // Those lines are needed to keep the conventions define in vpMeLine
167 // (Those in vpLine are less restrictive) Another way to have the
168 // coordinates of the desired features is to learn them before executing
169 // the program.
170 lined[0].setRho(-fabs(lined[0].getRho()));
171 lined[0].setTheta(0);
172 lined[1].setRho(-fabs(lined[1].getRho()));
173 lined[1].setTheta(M_PI);
174
175 vpFeatureLine pd[nbline];
176 vpFeatureBuilder::create(pd[0], lined[0]);
177 vpFeatureBuilder::create(pd[1], lined[1]);
178
179 vpTRACE("define the task");
180 vpTRACE("\t we want an eye-in-hand control law");
181 vpTRACE("\t robot is controlled in the camera frame");
183
184 vpTRACE("\t we want to see a point on a point..");
185 std::cout << std::endl;
186 for (i = 0; i < nbline; i++)
187 task.addFeature(p[i], pd[i]);
188
189 vpTRACE("\t set the gain");
190 task.setLambda(0.2);
191
192 vpTRACE("Display task information ");
193 task.print();
194
196
197 unsigned int iter = 0;
198 vpTRACE("\t loop");
199 vpColVector v;
200
202 double lambda_av = 0.05;
203 double alpha = 0.2;
204 double beta = 3;
205
206 for (;;) {
207 std::cout << "---------------------------------------------" << iter << std::endl;
208
209 try {
210 g.acquire(I);
212
213 // Track the lines and update the features
214 for (i = 0; i < nbline; i++) {
215 line[i].track(I);
216 line[i].display(I, vpColor::red);
217
218 vpFeatureBuilder::create(p[i], cam, line[i]);
219
220 p[i].display(cam, I, vpColor::red);
221 pd[i].display(cam, I, vpColor::green);
222 }
223
225
226 // Adaptative gain
227 double gain;
228 {
229 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
230 gain = lambda_av;
231 else {
232 gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
233 }
234 }
235 task.setLambda(gain);
236
237 v = task.computeControlLaw();
238
239 if (iter == 0)
242 }
243 catch (...) {
244 v = 0;
246 robot.stopMotion();
247 exit(1);
248 }
249
250 vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
251 iter++;
252 }
253
254 vpTRACE("Display task information ");
255 task.print();
256 return EXIT_SUCCESS;
257 }
258 catch (const vpException &e) {
259 std::cout << "Test failed with exception: " << e << std::endl;
260 return EXIT_FAILURE;
261 }
262}
263
264#else
265int main()
266{
267 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
268 return EXIT_SUCCESS;
269}
270
271#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
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 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
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition vpLine.h:100
void setRho(double rho)
Definition vpLine.h:152
void setTheta(double theta)
Definition vpLine.h:162
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)
Definition vpLine.cpp:82
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
@ 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
vpColVector computeControlLaw()
Definition vpServo.cpp:930
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
#define vpTRACE
Definition vpDebug.h:411