56 #include <visp3/core/vpConfig.h>
57 #include <visp3/core/vpDebug.h>
59 #if ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) \
60 && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) \
61 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
70 #include <visp3/core/vpCameraParameters.h>
71 #include <visp3/core/vpHomogeneousMatrix.h>
72 #include <visp3/core/vpImage.h>
73 #include <visp3/core/vpImagePoint.h>
74 #include <visp3/core/vpIoTools.h>
75 #include <visp3/core/vpMath.h>
76 #include <visp3/core/vpMeterPixelConversion.h>
77 #include <visp3/gui/vpDisplayGDI.h>
78 #include <visp3/gui/vpDisplayGTK.h>
79 #include <visp3/gui/vpDisplayX.h>
80 #include <visp3/io/vpParseArgv.h>
81 #include <visp3/robot/vpSimulatorViper850.h>
82 #include <visp3/visual_features/vpFeatureBuilder.h>
83 #include <visp3/visual_features/vpFeaturePoint.h>
84 #include <visp3/vs/vpServo.h>
87 #define GETOPTARGS "cdh"
89 void usage(
const char *name,
const char *badparam);
90 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
100 void usage(
const char *name,
const char *badparam)
103 Tests a control law with the following characteristics:\n\
104 - eye-in-hand control\n\
105 - articular velocity are computed\n\
106 - servo on 4 points,\n\
107 - internal and external camera view displays.\n\
110 %s [-c] [-d] [-h]\n", name);
115 Disable the mouse click. Useful to automaze the \n\
116 execution of this program without humain intervention.\n\
119 Turn off the display.\n\
125 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
139 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
147 click_allowed =
false;
153 usage(argv[0], NULL);
158 usage(argv[0], optarg_);
164 if ((c == 1) || (c == -1)) {
166 usage(argv[0], NULL);
167 std::cerr <<
"ERROR: " << std::endl;
168 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
175 int main(
int argc,
const char **argv)
178 bool opt_click_allowed =
true;
179 bool opt_display =
true;
182 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
188 #if defined VISP_HAVE_X11
190 #elif defined VISP_HAVE_GDI
192 #elif defined VISP_HAVE_OPENCV
201 displayInt.
init(Iint, 700, 0,
"Internal view");
206 std::cout << std::endl;
207 std::cout <<
"----------------------------------------------" << std::endl;
208 std::cout <<
" Test program for vpServo " << std::endl;
209 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
210 std::cout <<
" Simulation " << std::endl;
211 std::cout <<
" task : servo 4 points " << std::endl;
212 std::cout <<
"----------------------------------------------" << std::endl;
213 std::cout << std::endl;
227 for (
unsigned int i = 0; i < 4; i++)
232 for (
unsigned int i = 0; i < 4; i++)
242 for (
unsigned int i = 0; i < 4; i++)
243 point[i].track(cdMo);
245 for (
unsigned int i = 0; i < 4; i++)
255 for (
unsigned int i = 0; i < 4; i++)
273 robot.initialiseObjectRelativeToCamera(cMo);
276 robot.setDesiredCameraPosition(cdMo);
280 robot.getCameraParameters(cam, Iint);
285 robot.getInternalView(Iint);
292 unsigned int iter = 0;
294 while (iter++ < 500) {
295 std::cout <<
"---------------------------------------------" << iter << std::endl;
302 cMo = robot.get_cMo();
305 std::cout <<
"Initial robot position with respect to the object frame:\n";
310 for (
unsigned int i = 0; i < 4; i++) {
319 robot.getInternalView(Iint);
323 if (opt_display && opt_click_allowed && iter == 1) {
325 std::cout <<
"Click in the internal view window to continue..." << std::endl;
335 std::cout <<
"|| s - s* || " << (task.
getError()).sumSquare() << std::endl;
344 std::cout <<
"Final robot position with respect to the object frame:\n";
347 if (opt_display && opt_click_allowed) {
349 std::cout <<
"Click in the internal view window to end..." << std::endl;
355 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
359 #elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
362 std::cout <<
"You do not have X11, or GDI (Graphical Device Interface) of OpenCV functionalities to display images..." << std::endl;
363 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
364 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
365 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
366 std::cout <<
"- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
369 #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
372 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
378 std::cout <<
"You do not have threading capabilities" << std::endl;
379 std::cout <<
"Tip:" << std::endl;
380 std::cout <<
"- Install pthread, configure again ViSP using cmake and build again this example" << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Implementation of column vector and the associated operations.
Display for windows using GDI (available on any windows 32 platform).
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...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
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 point visual feature which is composed by two parameters that are the cartes...
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void setWorldCoordinates(double oX, double oY, double oZ)
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)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Simulator of Irisa's Viper S850 robot named Viper850.
@ TOOL_PTGREY_FLEA2_CAMERA
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()