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/vpSimulatorAfma6.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)
197 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" << std::endl;
206 std::cout <<
" Simulation " << std::endl;
207 std::cout <<
" task : servo 4 points " << std::endl;
208 std::cout <<
"----------------------------------------------" << std::endl;
209 std::cout << std::endl;
223 for (
unsigned int i = 0; i < 4; i++)
228 for (
unsigned int i = 0; i < 4; i++)
238 for (
unsigned int i = 0; i < 4; i++)
239 point[i].track(cdMo);
241 for (
unsigned int i = 0; i < 4; i++)
251 for (
unsigned int i = 0; i < 4; i++)
269 robot.initialiseObjectRelativeToCamera(cMo);
272 robot.setDesiredCameraPosition(cdMo);
276 robot.getCameraParameters(cam, Iint);
281 robot.getInternalView(Iint);
288 unsigned int iter = 0;
290 while (iter++ < 500) {
291 std::cout <<
"---------------------------------------------" << iter << std::endl;
298 cMo = robot.get_cMo();
301 std::cout <<
"Initial robot position with respect to the object frame:\n";
306 for (
unsigned int i = 0; i < 4; i++) {
315 robot.getInternalView(Iint);
319 if (opt_display && opt_click_allowed && iter == 1) {
321 std::cout <<
"Click in the internal view window to continue..." << std::endl;
331 std::cout <<
"|| s - s* || " << (task.
getError()).sumSquare() << std::endl;
340 std::cout <<
"Final robot position with respect to the object frame:\n";
343 if (opt_display && opt_click_allowed) {
345 std::cout <<
"Click in the internal view window to end..." << std::endl;
351 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 gantry robot named Afma6.
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()