53 #include <visp3/core/vpConfig.h>
54 #include <visp3/core/vpDebug.h>
56 #if defined(VISP_HAVE_THREADS) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) \
57 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
66 #include <visp3/core/vpCameraParameters.h>
67 #include <visp3/core/vpHomogeneousMatrix.h>
68 #include <visp3/core/vpImage.h>
69 #include <visp3/core/vpImagePoint.h>
70 #include <visp3/core/vpIoTools.h>
71 #include <visp3/core/vpMath.h>
72 #include <visp3/core/vpMeterPixelConversion.h>
73 #include <visp3/gui/vpDisplayGDI.h>
74 #include <visp3/gui/vpDisplayGTK.h>
75 #include <visp3/gui/vpDisplayX.h>
76 #include <visp3/io/vpParseArgv.h>
77 #include <visp3/robot/vpSimulatorViper850.h>
78 #include <visp3/visual_features/vpFeatureBuilder.h>
79 #include <visp3/visual_features/vpFeaturePoint.h>
80 #include <visp3/vs/vpServo.h>
83 #define GETOPTARGS "cdh"
85 void usage(
const char *name,
const char *badparam);
86 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
96 void usage(
const char *name,
const char *badparam)
99 Tests a control law with the following characteristics:\n\
100 - eye-in-hand control\n\
101 - articular velocity are computed\n\
102 - servo on 4 points,\n\
103 - internal and external camera view displays.\n\
106 %s [-c] [-d] [-h]\n",
112 Disable the mouse click. Useful to automate the \n\
113 execution of this program without human intervention.\n\
116 Turn off the display.\n\
122 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
136 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
144 click_allowed =
false;
150 usage(argv[0],
nullptr);
155 usage(argv[0], optarg_);
161 if ((c == 1) || (c == -1)) {
163 usage(argv[0],
nullptr);
164 std::cerr <<
"ERROR: " << std::endl;
165 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
172 int main(
int argc,
const char **argv)
175 bool opt_click_allowed =
true;
176 bool opt_display =
true;
179 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
185 #if defined(VISP_HAVE_X11)
187 #elif defined(VISP_HAVE_GDI)
189 #elif defined(HAVE_OPENCV_HIGHGUI)
198 displayInt.
init(Iint, 700, 0,
"Internal view");
203 std::cout << std::endl;
204 std::cout <<
"----------------------------------------------" << std::endl;
205 std::cout <<
" Test program for vpServo " << std::endl;
206 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
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;
224 for (
unsigned int i = 0; i < 4; i++)
229 for (
unsigned int i = 0; i < 4; i++)
239 for (
unsigned int i = 0; i < 4; i++)
240 point[i].track(cdMo);
242 for (
unsigned int i = 0; i < 4; i++)
252 for (
unsigned int i = 0; i < 4; i++)
270 robot.initialiseObjectRelativeToCamera(cMo);
273 robot.setDesiredCameraPosition(cdMo);
277 robot.getCameraParameters(cam, Iint);
282 robot.getInternalView(Iint);
289 unsigned int iter = 0;
291 while (iter++ < 500) {
292 std::cout <<
"---------------------------------------------" << iter << std::endl;
299 cMo = robot.get_cMo();
302 std::cout <<
"Initial robot position with respect to the object frame:\n";
307 for (
unsigned int i = 0; i < 4; i++) {
316 robot.getInternalView(Iint);
320 if (opt_display && opt_click_allowed && iter == 1) {
322 std::cout <<
"Click in the internal view window to continue..." << std::endl;
332 std::cout <<
"|| s - s* || " << (task.
getError()).sumSquare() << std::endl;
341 std::cout <<
"Final robot position with respect to the object frame:\n";
344 if (opt_display && opt_click_allowed) {
346 std::cout <<
"Click in the internal view window to end..." << std::endl;
352 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
356 #elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
359 std::cout <<
"You do not have X11, or GDI (Graphical Device Interface) of OpenCV functionalities to display images..."
361 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
362 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
363 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
364 std::cout <<
"- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
367 #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
370 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
376 std::cout <<
"You do not have threading capabilities" << std::endl;
377 std::cout <<
"Tip:" << std::endl;
378 std::cout <<
"- Install pthread, configure again ViSP using cmake and build again this example" << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
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="") vp_override
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 emitted 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) vp_override
@ 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 addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
Simulator of Irisa's Viper S850 robot named Viper850.
@ TOOL_PTGREY_FLEA2_CAMERA
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()