Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6Ellipse2DCamVelocity.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
56#include <cmath> // std::fabs
57#include <limits> // numeric_limits
58#include <stdlib.h>
59#include <visp3/core/vpConfig.h>
60#include <visp3/core/vpDebug.h> // Debug trace
61#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
62
63#include <visp3/core/vpDisplay.h>
64#include <visp3/core/vpImage.h>
65#include <visp3/gui/vpDisplayGTK.h>
66#include <visp3/gui/vpDisplayOpenCV.h>
67#include <visp3/gui/vpDisplayX.h>
68#include <visp3/sensor/vp1394TwoGrabber.h>
69
70#include <visp3/core/vpHomogeneousMatrix.h>
71#include <visp3/core/vpMath.h>
72#include <visp3/visual_features/vpFeatureBuilder.h>
73#include <visp3/visual_features/vpFeatureEllipse.h>
74#include <visp3/vs/vpServo.h>
75
76#include <visp3/robot/vpRobotAfma6.h>
77
78// Exception
79#include <visp3/core/vpException.h>
80#include <visp3/vs/vpServoDisplay.h>
81
82#include <visp3/blob/vpDot.h>
83
84int main()
85{
86 try {
87 vpServo task;
88
93 g.open(I);
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 std::cout << std::endl;
108 std::cout << "-------------------------------------------------------" << std::endl;
109 std::cout << " Test program for vpServo " << std::endl;
110 std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
111 std::cout << " Simulation " << std::endl;
112 std::cout << " task : servo a point " << std::endl;
113 std::cout << "-------------------------------------------------------" << std::endl;
114 std::cout << std::endl;
115
116 vpDot dot;
117
118 dot.setMaxDotSize(0.30); // Max dot size is 30 % of the image size
119 // dot.setGraphics(true) ;
120 dot.setComputeMoments(true);
121 std::cout << "Click on an ellipse..." << std::endl;
122 dot.initTracking(I);
123 vpImagePoint cog = dot.getCog();
126
127 dot.track(I);
128
130
131 vpRobotAfma6 robot;
132
133 // Update camera parameters
134 robot.getCameraParameters(cam, I);
135
136 vpTRACE("sets the current position of the visual feature ");
138 vpFeatureBuilder::create(c, cam, dot);
139
140 std::cout << " Learning 0/1 " << std::endl;
141 int learning;
142 std::cin >> learning;
143 std::string name = "dat/ellipse.dat";
144 if (learning == 1) {
145 // save the object position
146 vpTRACE("Save the location of the object in a file dat/ellipse.dat");
147 std::ofstream f(name.c_str());
148 f << c.get_s().t();
149 f.close();
150 exit(1);
151 }
152
153 vpTRACE("sets the desired position of the visual feature ");
155 std::ifstream f("dat/ellipse.dat");
156 double x, y, n20, n11, n02;
157 f >> x;
158 f >> y;
159 f >> n20;
160 f >> n11;
161 f >> n02;
162 f.close();
163 cd.buildFrom(x, y, n20, n11, n02);
164 cd.setABC(0, 0, 10);
165
168
169 task.addFeature(c, cd);
170
171 task.setLambda(0.01);
172
174 unsigned int iter = 0;
175 double lambda_av = 0.01;
176 double alpha = 0.1; // 1 ;
177 double beta = 3; // 3 ;
178
179 std::cout << "alpha 0.7" << std::endl;
180 std::cin >> alpha;
181 std::cout << "beta 5" << std::endl;
182 std::cin >> beta;
183 for (;;) {
184 std::cout << "---------------------------------------------" << iter++ << std::endl;
185
186 g.acquire(I);
188
189 dot.track(I);
190
191 // Get the dot cog
192 cog = dot.getCog();
193
195
196 vpFeatureBuilder::create(c, cam, dot);
197 // Compute the adaptative gain (speed up the convergence)
198 double gain;
199 if (iter > 2) {
200 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
201 gain = lambda_av;
202 else {
203 gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
204 }
205 } else
206 gain = lambda_av;
207
208 vpTRACE("%f %f", (task.getError()).sumSquare(), gain);
209 task.setLambda(gain);
210 vpColVector v;
211 v = task.computeControlLaw();
212 std::cout << "rank " << task.getTaskRank() << std::endl;
213 vpServoDisplay::display(task, cam, I);
214 std::cout << v.t();
216
218 vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
219 }
220
221 vpTRACE("Display task information ");
222 task.print();
223 return EXIT_SUCCESS;
224 } catch (const vpException &e) {
225 std::cout << "Test failed with exception: " << e << std::endl;
226 return EXIT_FAILURE;
227 }
228}
229
230#else
231int main()
232{
233 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
234 return EXIT_SUCCESS;
235}
236#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)
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
vpRowVector t() const
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 setMaxDotSize(double percentage)
Definition vpDot.cpp:582
void initTracking(const vpImage< unsigned char > &I)
Definition vpDot.cpp:617
void setComputeMoments(bool activate)
Definition vpDot.h:333
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 2D ellipse visual feature.
void setABC(double A, double B, double C)
void buildFrom(double x, double y, double n20, double n11, double n02)
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
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
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_CAMERA
Definition vpServo.h:151
unsigned int getTaskRank() const
Definition vpServo.cpp:1796
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
@ CURRENT
Definition vpServo.h:179
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
#define vpTRACE
Definition vpDebug.h:411