Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6Line2DCamVelocity.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
54#include <stdlib.h>
55#include <visp3/core/vpConfig.h>
56#include <visp3/core/vpDebug.h> // Debug trace
57#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
58
59#include <visp3/core/vpDisplay.h>
60#include <visp3/core/vpImage.h>
61#include <visp3/gui/vpDisplayGTK.h>
62#include <visp3/gui/vpDisplayOpenCV.h>
63#include <visp3/gui/vpDisplayX.h>
64#include <visp3/sensor/vp1394TwoGrabber.h>
65
66#include <visp3/core/vpHomogeneousMatrix.h>
67#include <visp3/core/vpLine.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
89 g.open(I);
90
91 g.acquire(I);
92
93#ifdef VISP_HAVE_X11
94 vpDisplayX display(I, 100, 100, "Current image");
95#elif defined(HAVE_OPENCV_HIGHGUI)
96 vpDisplayOpenCV display(I, 100, 100, "Current image");
97#elif defined(VISP_HAVE_GTK)
98 vpDisplayGTK display(I, 100, 100, "Current image");
99#endif
100
103
104 vpServo task;
105
106 std::cout << std::endl;
107 std::cout << "-------------------------------------------------------" << std::endl;
108 std::cout << " Test program for vpServo " << std::endl;
109 std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
110 std::cout << " Simulation " << std::endl;
111 std::cout << " task : servo a line " << std::endl;
112 std::cout << "-------------------------------------------------------" << std::endl;
113 std::cout << std::endl;
114
115 vpMeLine line;
116
117 vpMe me;
118 me.setRange(10);
119 me.setPointsToTrack(100);
121 me.setThreshold(50);
122 me.setSampleStep(10);
124
125 line.setMe(&me);
126
127 // Initialize the tracking. Define the line to track.
128 line.initTracking(I);
129 line.track(I);
130
131 vpRobotAfma6 robot;
132 // robot.move("pos-init.pos") ;
133
135 // Update camera parameters
136 robot.getCameraParameters(cam, I);
137
138 vpTRACE("sets the current position of the visual feature ");
140 vpFeatureBuilder::create(p, cam, line);
141
142 vpTRACE("sets the desired position of the visual feature ");
143 vpLine lined;
144 lined.setWorldCoordinates(1, 0, 0, 0, 0, 0, 1, 0);
145 vpHomogeneousMatrix cMo(0, 0, 0.3, 0, 0, vpMath::rad(0));
146 lined.project(cMo);
147 lined.setRho(-fabs(lined.getRho()));
148 lined.setTheta(0);
149
150 vpFeatureLine pd;
151 vpFeatureBuilder::create(pd, lined);
152
153 vpTRACE("define the task");
154 vpTRACE("\t we want an eye-in-hand control law");
155 vpTRACE("\t robot is controlled in the camera frame");
157
158 vpTRACE("\t we want to see a point on a point..");
159 std::cout << std::endl;
160 task.addFeature(p, pd);
161
162 vpTRACE("\t set the gain");
163 task.setLambda(0.2);
164
165 vpTRACE("Display task information ");
166 task.print();
167
169
170 unsigned int iter = 0;
171 vpTRACE("\t loop");
172 vpColVector v;
173 for (;;) {
174 std::cout << "---------------------------------------------" << iter << std::endl;
175
176 try {
177 g.acquire(I);
179
180 // Track the line
181 line.track(I);
182 line.display(I, vpColor::red);
183
184 // Update the current line feature
185 vpFeatureBuilder::create(p, cam, line);
186
187 // displqy the current and the desired features
188 p.display(cam, I, vpColor::red);
189 pd.display(cam, I, vpColor::green);
190
191 v = task.computeControlLaw();
192
194 if (iter == 0)
197 }
198 catch (...) {
199 v = 0;
201 robot.stopMotion();
202 exit(1);
203 }
204
205 vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
206 iter++;
207 }
208
209 vpTRACE("Display task information ");
210 task.print();
211 return EXIT_SUCCESS;
212 }
213 catch (const vpException &e) {
214 std::cout << "Test failed with exception: " << e << std::endl;
215 return EXIT_FAILURE;
216 }
217}
218
219#else
220int main()
221{
222 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
223 return EXIT_SUCCESS;
224}
225
226#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
double getRho() const
Definition vpLine.h:129
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