62 #include <visp/vpDebug.h>
63 #include <visp/vpConfig.h>
65 #if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
70 #include <visp/vpMath.h>
71 #include <visp/vpHomogeneousMatrix.h>
72 #include <visp/vpServo.h>
73 #include <visp/vpRobotCamera.h>
74 #include <visp/vpDebug.h>
75 #include <visp/vpFeatureBuilder.h>
76 #include <visp/vpFeaturePointPolar.h>
78 #include <visp/vpServoDisplay.h>
79 #include <visp/vpProjectionDisplay.h>
80 #include <visp/vpMeterPixelConversion.h>
82 #include <visp/vpImage.h>
83 #include <visp/vpImagePoint.h>
84 #include <visp/vpDisplayX.h>
85 #include <visp/vpDisplayGTK.h>
86 #include <visp/vpDisplayGDI.h>
87 #include <visp/vpCameraParameters.h>
88 #include <visp/vpParseArgv.h>
89 #include <visp/vpIoTools.h>
92 #define GETOPTARGS "cdh"
102 void usage(
const char *name,
const char *badparam)
105 Tests a control law with the following characteristics:\n\
106 - eye-in-hand control\n\
107 - articular velocity are computed\n\
108 - servo on 4 points,\n\
109 - internal and external camera view displays.\n\
112 %s [-c] [-d] [-h]\n", name);
117 Disable the mouse click. Useful to automaze the \n\
118 execution of this program without humain intervention.\n\
121 Turn off the display.\n\
127 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
141 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
148 case 'c': click_allowed =
false;
break;
149 case 'd': display =
false;
break;
150 case 'h': usage(argv[0], NULL);
return false;
break;
153 usage(argv[0], optarg);
158 if ((c == 1) || (c == -1)) {
160 usage(argv[0], NULL);
161 std::cerr <<
"ERROR: " << std::endl;
162 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
170 main(
int argc,
const char ** argv)
179 std::string username;
184 std::string logdirname;
185 logdirname =
"C://" + username;
194 std::cerr << std::endl
195 <<
"ERROR:" << std::endl;
196 std::cerr <<
" Cannot create " << logdirname << std::endl;
200 std::string logfilename;
201 logfilename = logdirname +
"/log.dat";
204 std::ofstream flog(logfilename.c_str());
207 bool opt_click_allowed =
true;
208 bool opt_display =
true;
211 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
217 #if defined VISP_HAVE_X11
220 #elif defined VISP_HAVE_GTK
223 #elif defined VISP_HAVE_GDI
234 displayInt.
init(Iint,0,0,
"Internal view") ;
235 displayExt.
init(Iext,330,000,
"External view") ;
240 double px, py ; px = py = 500 ;
241 double u0, v0 ; u0 = 150, v0 = 160 ;
250 std::cout << std::endl ;
251 std::cout <<
"----------------------------------------------" << std::endl ;
252 std::cout <<
" Test program for vpServo " <<std::endl ;
253 std::cout <<
" Eye-in-hand task control, articular velocity are computed"
255 std::cout <<
" Simulation " << std::endl ;
256 std::cout <<
" task : servo 4 points " << std::endl ;
257 std::cout <<
"----------------------------------------------" << std::endl ;
258 std::cout << std::endl ;
267 #if defined(TRANS_Z_PURE)
274 #elif defined(TRANS_X_PURE)
282 #elif defined(ROT_Z_PURE)
290 #elif defined(ROT_X_PURE)
298 #elif defined(COMPLEX)
306 #elif defined(PROBLEM)
330 for (i = 0 ; i < 4 ; i++)
331 externalview.
insert(point[i]) ;
338 for (i = 0 ; i < 4 ; i++) {
339 point[i].
track(cMod);
348 for (i = 0 ; i < 4 ; i++)
349 point[i].track(cMo) ;
353 for (i = 0 ; i < 4 ; i++) {
379 for (i = 0 ; i < 4 ; i++)
386 std::cout <<
"\nDisplay task information: " << std::endl;
389 unsigned int iter=0 ;
391 while(iter++ < 200) {
392 std::cout <<
"---------------------------------------------"
393 << iter <<std::endl ;
405 std::cout <<
"Initial robot position with respect to the object frame:\n";
410 for (i = 0 ; i < 4 ; i++) {
411 point[i].
track(cMo) ;
430 std::cout <<
"Display task information: " << std::endl;
442 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" "
443 << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
445 std::cout <<
"v: " << v.
t() << std::endl;
447 std::cout <<
"|| s - s* || = "<< ( task.
getError() ).sumSquare() << std::endl;
452 flog << ( task.
getError() ).t() <<
" ";
453 std::cout <<
"Error (s-s*): " << ( task.
getError() ).t() <<
"\n";
456 for (i = 0 ; i < 4 ; i++) {
460 for (i = 0 ; i < 4 ; i++) {
461 flog << point[i].
get_x() <<
" " << point[i].
get_y() <<
" ";
487 std::cout <<
"Final robot position with respect to the object frame:\n";
490 if (opt_display && opt_click_allowed) {
492 std::cout <<
"\n\nClick in the internal view to end..." << std::endl;
500 vpERROR_TRACE(
"You do not have X11, GTK or GDI display functionalities...");
void set_j(const double j)
Definition of the vpMatrix class.
static void display(vpServo &s, const vpCameraParameters &cam, vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void print()
Print the matrix as a vector [T thetaU].
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Display for windows using GDI (available on any windows 32 platform).
Define the X11 console to display images.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
void set_i(const double i)
void setLambda(double _lambda)
set the gain lambda
void track(const vpHomogeneousMatrix &cMo)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
double get_y() const
Get the point y coordinate in the image plane.
static const vpColor green
void set_cVe(vpVelocityTwistMatrix &_cVe)
static void flush(const vpImage< unsigned char > &I)
Class that defines 2D image point visual feature with polar coordinates .
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a point.
void kill()
destruction (memory deallocation if required)
vpColVector getError() const
vpColVector computeControlLaw()
compute the desired control law
Class that defines the simplest robot: a free flying camera.
static void display(const vpImage< unsigned char > &I)
void set_eJe(vpMatrix &_eJe)
vpRowVector t() const
transpose of Vector
Generic class defining intrinsic camera parameters.
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
double get_x() const
Get the point x coordinate in the image plane.
void insert(vpForwardProjection &fp)
void getPosition(vpColVector &q)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setPosition(const vpRobot::vpControlFrameType, const vpColVector &)
Set a displacement (frame has to be specified) in position control.
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor color, const bool &displayTraj=false)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
static double rad(double deg)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color=vpColor::green)=0
void get_eJe(vpMatrix &_eJe)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
virtual bool getClick(bool blocking=true)=0
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law.
interface with the image for feature display
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...