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/vpCameraParameters.h>
73 #include <visp/vpDisplayX.h>
74 #include <visp/vpDisplayGTK.h>
75 #include <visp/vpDisplayGDI.h>
76 #include <visp/vpFeatureBuilder.h>
77 #include <visp/vpFeaturePoint.h>
78 #include <visp/vpHomogeneousMatrix.h>
79 #include <visp/vpImage.h>
80 #include <visp/vpImagePoint.h>
81 #include <visp/vpIoTools.h>
82 #include <visp/vpMath.h>
83 #include <visp/vpMeterPixelConversion.h>
84 #include <visp/vpParseArgv.h>
85 #include <visp/vpServo.h>
86 #include <visp/vpSimulatorViper850.h>
89 #define GETOPTARGS "cdh"
99 void usage(
const char *name,
const char *badparam)
102 Tests a control law with the following characteristics:\n\
103 - eye-in-hand control\n\
104 - articular velocity are computed\n\
105 - servo on 4 points,\n\
106 - internal and external camera view displays.\n\
109 %s [-c] [-d] [-h]\n", name);
114 Disable the mouse click. Useful to automaze the \n\
115 execution of this program without humain intervention.\n\
118 Turn off the display.\n\
124 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
138 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
145 case 'c': click_allowed =
false;
break;
146 case 'd': display =
false;
break;
147 case 'h': usage(argv[0], NULL);
return false;
break;
150 usage(argv[0], optarg);
155 if ((c == 1) || (c == -1)) {
157 usage(argv[0], NULL);
158 std::cerr <<
"ERROR: " << std::endl;
159 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
167 main(
int argc,
const char ** argv)
169 bool opt_click_allowed =
true;
170 bool opt_display =
true;
173 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
179 #if defined VISP_HAVE_X11
181 #elif defined VISP_HAVE_GDI
183 #elif defined VISP_HAVE_OPENCV
192 displayInt.
init(Iint,700,0,
"Internal view") ;
198 std::cout << std::endl ;
199 std::cout <<
"----------------------------------------------" << std::endl ;
200 std::cout <<
" Test program for vpServo " <<std::endl ;
201 std::cout <<
" Eye-in-hand task control, articular velocity are computed"
203 std::cout <<
" Simulation " << std::endl ;
204 std::cout <<
" task : servo 4 points " << std::endl ;
205 std::cout <<
"----------------------------------------------" << std::endl ;
206 std::cout << std::endl ;
221 for (i = 0 ; i < 4 ; i++)
222 point[i].track(cMo) ;
226 for (i = 0 ; i < 4 ; i++)
236 for (
int i = 0 ; i < 4 ; i++)
237 point[i].track(cdMo);
239 for (
int i = 0 ; i < 4 ; i++)
249 for (i = 0 ; i < 4 ; i++)
266 robot.initialiseObjectRelativeToCamera(cMo);
269 robot.setDesiredCameraPosition(cdMo);
273 robot.getCameraParameters(cam,Iint);
279 robot.getInternalView(Iint);
286 unsigned int iter=0 ;
290 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
297 cMo = robot.get_cMo();
300 std::cout <<
"Initial robot position with respect to the object frame:\n";
305 for (i = 0 ; i < 4 ; i++)
307 point[i].
track(cMo) ;
322 robot.getInternalView(Iint);
326 if (opt_display && opt_click_allowed && iter == 1)
329 std::cout <<
"Click in the internal view window to continue..." << std::endl;
339 std::cout <<
"|| s - s* || " << ( task.
getError() ).sumSquare() <<std::endl ;
349 std::cout <<
"Final robot position with respect to the object frame:\n";
352 if (opt_display && opt_click_allowed)
355 std::cout <<
"Click in the internal view window to end..." << std::endl;
363 vpERROR_TRACE(
"You do not have X11, OpenCV or GDI display functionalities or threading capabilities...");
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
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...
A 40cm by 40cm plate with 4 points at coordinates (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each point is represented by a circle with 2cm radius.
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...
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.
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)
Simulator of Irisa's Viper S850 robot named Viper850.
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 descbribed in and .
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
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...