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/vpSimulatorAfma6.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 200 displayInt.
init(Iint, 700, 0,
"Internal view");
205 std::cout << std::endl;
206 std::cout <<
"----------------------------------------------" << std::endl;
207 std::cout <<
" Test program for vpServo " << std::endl;
208 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
209 std::cout <<
" Simulation " << std::endl;
210 std::cout <<
" task : servo 4 points " << std::endl;
211 std::cout <<
"----------------------------------------------" << std::endl;
212 std::cout << std::endl;
226 for (
unsigned int i = 0; i < 4; i++)
231 for (
unsigned int i = 0; i < 4; i++)
241 for (
unsigned int i = 0; i < 4; i++)
242 point[i].track(cdMo);
244 for (
unsigned int i = 0; i < 4; i++)
254 for (
unsigned int i = 0; i < 4; i++)
272 robot.initialiseObjectRelativeToCamera(cMo);
275 robot.setDesiredCameraPosition(cdMo);
279 robot.getCameraParameters(cam, Iint);
284 robot.getInternalView(Iint);
291 unsigned int iter = 0;
293 while (iter++ < 500) {
294 std::cout <<
"---------------------------------------------" << iter << std::endl;
301 cMo = robot.get_cMo();
304 std::cout <<
"Initial robot position with respect to the object frame:\n";
309 for (
unsigned int i = 0; i < 4; i++) {
318 robot.getInternalView(Iint);
322 if (opt_display && opt_click_allowed && iter == 1) {
324 std::cout <<
"Click in the internal view window to continue..." << std::endl;
334 std::cout <<
"|| s - s* || " << (task.
getError()).sumSquare() << std::endl;
343 std::cout <<
"Final robot position with respect to the object frame:\n";
346 if (opt_display && opt_click_allowed) {
348 std::cout <<
"Click in the internal view window to end..." << std::endl;
354 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;
VISP_EXPORT int wait(double t0, double t)
void setWorldCoordinates(double oX, double oY, double oZ)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Display for windows using GDI (available on any windows 32 platform).
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
error that can be emited by ViSP classes.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void track(const vpHomogeneousMatrix &cMo)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Initialize the velocity controller.
vpColVector getError() const
vpColVector computeControlLaw()
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
Simulator of Irisa's gantry robot named Afma6.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
Implementation of column vector and the associated operations.
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)