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