68 #include <visp/vpDebug.h>
69 #include <visp/vpConfig.h>
71 #if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
76 #include <visp/vpMath.h>
77 #include <visp/vpHomogeneousMatrix.h>
78 #include <visp/vpFeaturePoint.h>
79 #include <visp/vpServo.h>
80 #include <visp/vpRobotCamera.h>
81 #include <visp/vpDebug.h>
82 #include <visp/vpFeatureBuilder.h>
84 #include <visp/vpServoDisplay.h>
85 #include <visp/vpProjectionDisplay.h>
87 #include <visp/vpImage.h>
88 #include <visp/vpDisplayX.h>
89 #include <visp/vpDisplayGTK.h>
90 #include <visp/vpDisplayGDI.h>
91 #include <visp/vpCameraParameters.h>
92 #include <visp/vpParseArgv.h>
95 #define GETOPTARGS "cdh"
105 void usage(
const char *name,
const char *badparam)
108 Tests a control law with the following characteristics:\n\
109 - eye-in-hand control\n\
110 - articular velocity are computed\n\
111 - servo on 4 points,\n\
112 - internal and external camera view displays.\n\
115 %s [-c] [-d] [-h]\n", name);
120 Disable the mouse click. Useful to automaze the \n\
121 execution of this program without humain intervention.\n\
124 Turn off the display.\n\
130 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
144 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
151 case 'c': click_allowed =
false;
break;
152 case 'd': display =
false;
break;
153 case 'h': usage(argv[0], NULL);
return false;
break;
156 usage(argv[0], optarg);
161 if ((c == 1) || (c == -1)) {
163 usage(argv[0], NULL);
164 std::cerr <<
"ERROR: " << std::endl;
165 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
173 main(
int argc,
const char ** argv)
176 bool opt_click_allowed =
true;
177 bool opt_display =
true;
180 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
186 #if defined VISP_HAVE_X11
189 #elif defined VISP_HAVE_GTK
192 #elif defined VISP_HAVE_GDI
203 displayInt.
init(Iint,0,0,
"Internal view") ;
204 displayExt.
init(Iext,330,000,
"External view") ;
209 double px, py ; px = py = 500 ;
210 double u0, v0 ; u0 = 150, v0 = 160 ;
219 std::cout << std::endl ;
220 std::cout <<
"----------------------------------------------" << std::endl ;
221 std::cout <<
" Test program for vpServo " <<std::endl ;
222 std::cout <<
" Eye-in-hand task control, articular velocity are computed"
224 std::cout <<
" Simulation " << std::endl ;
225 std::cout <<
" task : servo 4 points " << std::endl ;
226 std::cout <<
"----------------------------------------------" << std::endl ;
227 std::cout << std::endl ;
230 vpTRACE(
"sets the initial camera location " ) ;
240 vpTRACE(
"sets the point coordinates in the object frame " ) ;
248 for (i = 0 ; i < 4 ; i++)
249 externalview.
insert(point[i]) ;
251 vpTRACE(
"project : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
252 for (i = 0 ; i < 4 ; i++)
253 point[i].track(cMo) ;
255 vpTRACE(
"sets the desired position of the point ") ;
257 for (i = 0 ; i < 4 ; i++)
261 vpTRACE(
"sets the desired position of the feature point s*") ;
270 vpTRACE(
"\t we want an eye-in-hand control law") ;
271 vpTRACE(
"\t articular velocity are computed") ;
276 vpTRACE(
"Set the position of the camera in the end-effector frame ") ;
281 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
286 vpTRACE(
"\t we want to see a point on a point..") ;
287 for (i = 0 ; i < 4 ; i++)
294 vpTRACE(
"Display task information " ) ;
297 unsigned int iter=0 ;
301 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
307 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
308 vpTRACE(
"since q is modified eJe is modified") ;
316 std::cout <<
"Initial robot position with respect to the object frame:\n";
320 if (iter==1)
vpTRACE(
"\t new point position ") ;
321 for (i = 0 ; i < 4 ; i++)
323 point[i].
track(cMo) ;
338 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
343 vpTRACE(
"Display task information " ) ;
347 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
350 vpTRACE(
"\t\t || s - s* || ") ;
351 std::cout << ( task.
getError() ).sumSquare() <<std::endl ;
354 vpTRACE(
"Display task information " ) ;
359 std::cout <<
"Final robot position with respect to the object frame:\n";
362 if (opt_display && opt_click_allowed) {
364 vpTRACE(
"\n\nClick in the internal view window to end...");
372 vpERROR_TRACE(
"You do not have X11, GTK or GDI display functionalities...");
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 setLambda(double _lambda)
set the gain lambda
void track(const vpHomogeneousMatrix &cMo)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static const vpColor green
void set_cVe(vpVelocityTwistMatrix &_cVe)
static void flush(const vpImage< unsigned char > &I)
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)
Generic class defining intrinsic camera parameters.
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
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)
void buildFrom(const double x, const double y, const double Z)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
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
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...