61 #include <visp3/core/vpConfig.h>
62 #include <visp3/core/vpDebug.h>
63 #if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
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>
73 #include <visp3/core/vpCylinder.h>
74 #include <visp3/core/vpHomogeneousMatrix.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>
81 #include <visp3/robot/vpRobotAfma6.h>
84 #include <visp3/core/vpException.h>
85 #include <visp3/vs/vpServoDisplay.h>
100 vpDisplayX display(I, 100, 100,
"Current image");
101 #elif defined(VISP_HAVE_OPENCV)
103 #elif defined(VISP_HAVE_GTK)
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;
132 for (i = 0; i < nbline; i++) {
145 robot.getCameraParameters(cam, I);
147 vpTRACE(
"sets the current position of the visual feature ");
149 for (i = 0; i < nbline; i++)
152 vpTRACE(
"sets the desired position of the visual feature ");
171 vpTRACE(
"\t we want an eye-in-hand control law");
172 vpTRACE(
"\t robot is controlled in the camera frame");
176 vpTRACE(
"\t we want to see a point on a point..");
177 std::cout << std::endl;
178 for (i = 0; i < nbline; i++)
184 vpTRACE(
"Display task information ");
189 unsigned int iter = 0;
193 double lambda_av = 0.05;
199 while (erreur > 0.00001) {
200 std::cout <<
"---------------------------------------------" << iter << std::endl;
207 for (i = 0; i < nbline; i++) {
222 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
225 gain = alpha * exp(-beta * (task.
getError()).sumSquare()) + lambda_av;
242 erreur = (task.
getError()).sumSquare();
259 double vitesse = 0.02;
260 unsigned int tempo = 1200;
263 std::cout <<
"---------------------------------------------" << iter << std::endl;
270 for (i = 0; i < nbline; i++) {
285 if (iter % tempo < 400 /*&& iter%tempo >= 0*/) {
287 e1[0] = fabs(vitesse);
289 rapport = vitesse / proj_e1[0];
297 if (iter % tempo < 600 && iter % tempo >= 400) {
299 e2[1] = fabs(vitesse);
301 rapport = vitesse / proj_e2[1];
306 if (iter % tempo < 1000 && iter % tempo >= 600) {
308 e1[0] = -fabs(vitesse);
310 rapport = -vitesse / proj_e1[0];
315 if (iter % tempo < 1200 && iter % tempo >= 1000) {
317 e2[1] = -fabs(vitesse);
319 rapport = -vitesse / proj_e2[1];
336 vpTRACE(
"Display task information ");
341 std::cout <<
"Test failed with exception: " << e << std::endl;
349 std::cout <<
"You do not have an afma6 robot connected to your computer..." << std::endl;
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
void setVideoMode(vp1394TwoVideoModeType videomode)
@ vpVIDEO_MODE_640x480_MONO8
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 green
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
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...
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 emited by ViSP classes.
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.
static double rad(double deg)
Class that tracks in an image a line moving edges.
void display(const vpImage< unsigned char > &I, vpColor col)
void track(const vpImage< unsigned char > &Im)
void initTracking(const vpImage< unsigned char > &I)
void setDisplay(vpMeSite::vpMeSiteDisplayType select)
void setSampleStep(const double &s)
void setRange(const unsigned int &r)
void setPointsToTrack(const int &n)
void setThreshold(const double &t)
Control of Irisa's gantry robot named Afma6.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)