60 #include <visp/vpDebug.h>
61 #include <visp/vpConfig.h>
63 #if (defined(WIN32) || defined(VISP_HAVE_PTHREAD)) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI))
72 #include <visp/vpMath.h>
73 #include <visp/vpHomogeneousMatrix.h>
74 #include <visp/vpServo.h>
75 #include <visp/vpDebug.h>
76 #include <visp/vpFeatureBuilder.h>
77 #include <visp/vpFeaturePoint.h>
78 #include <visp/vpSimulatorAfma6.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)
172 bool opt_click_allowed =
true;
173 bool opt_display =
true;
176 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
182 #if defined VISP_HAVE_X11
184 #elif defined VISP_HAVE_GDI
186 #elif defined VISP_HAVE_OPENCV
195 displayInt.
init(Iint,700,0,
"Internal view") ;
202 std::cout << std::endl ;
203 std::cout <<
"----------------------------------------------" << std::endl ;
204 std::cout <<
" Test program for vpServo " <<std::endl ;
205 std::cout <<
" Eye-in-hand task control, articular velocity are computed"
207 std::cout <<
" Simulation " << std::endl ;
208 std::cout <<
" task : servo 4 points " << std::endl ;
209 std::cout <<
"----------------------------------------------" << std::endl ;
210 std::cout << std::endl ;
213 vpTRACE(
"sets the initial camera location " ) ;
218 vpTRACE(
"sets the point coordinates in the object frame " ) ;
225 vpTRACE(
"project : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
226 for (i = 0 ; i < 4 ; i++)
227 point[i].track(cMo) ;
229 vpTRACE(
"sets the desired position of the point ") ;
231 for (i = 0 ; i < 4 ; i++)
234 vpTRACE(
"sets the desired position of the feature point s*") ;
241 for (
int i = 0 ; i < 4 ; i++)
242 point[i].track(cdMo);
244 for (
int i = 0 ; i < 4 ; i++)
248 vpTRACE(
"\t we want an eye-in-hand control law") ;
249 vpTRACE(
"\t articular velocity are computed") ;
253 vpTRACE(
"\t we want to see a point on a point..") ;
254 for (i = 0 ; i < 4 ; i++)
271 robot.initialiseObjectRelativeToCamera(cMo);
274 robot.setDesiredCameraPosition(cdMo);
278 robot.getCameraParameters(cam,Iint);
284 robot.getInternalView(Iint);
289 vpTRACE(
"Display task information " ) ;
292 unsigned int iter=0 ;
296 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
303 cMo = robot.get_cMo();
306 std::cout <<
"Initial robot position with respect to the object frame:\n";
310 if (iter==1)
vpTRACE(
"\t new point position ") ;
311 for (i = 0 ; i < 4 ; i++)
313 point[i].
track(cMo) ;
328 robot.getInternalView(Iint);
332 if (opt_display && opt_click_allowed && iter == 1)
335 vpTRACE(
"\n\nClick in the internal view window to continue...");
339 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
342 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
345 vpTRACE(
"\t\t || s - s* || ") ;
346 std::cout << ( task.
getError() ).sumSquare() <<std::endl ;
352 vpTRACE(
"Display task information " ) ;
357 std::cout <<
"Final robot position with respect to the object frame:\n";
360 if (opt_display && opt_click_allowed)
363 vpTRACE(
"\n\nClick in the internal view window to end...");
371 vpERROR_TRACE(
"You do not have X11, OpenCV or GDI display functionalities or threading capabilities...");
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
Perspective projection without distortion model.
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 double measureTimeMs()
static int wait(double t0, double t)
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
void kill()
destruction (memory deallocation if required)
Initialize the velocity controller.
vpColVector getError() const
vpColVector computeControlLaw()
compute the desired control law
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
Simulator of Irisa's gantry robot named Afma6.
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...
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.
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...