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)) 69 #include <visp3/core/vpCameraParameters.h> 70 #include <visp3/core/vpHomogeneousMatrix.h> 71 #include <visp3/core/vpImage.h> 72 #include <visp3/core/vpImagePoint.h> 73 #include <visp3/core/vpIoTools.h> 74 #include <visp3/core/vpMath.h> 75 #include <visp3/core/vpMeterPixelConversion.h> 76 #include <visp3/gui/vpDisplayGDI.h> 77 #include <visp3/gui/vpDisplayGTK.h> 78 #include <visp3/gui/vpDisplayX.h> 79 #include <visp3/io/vpParseArgv.h> 80 #include <visp3/robot/vpSimulatorAfma6.h> 81 #include <visp3/visual_features/vpFeatureBuilder.h> 82 #include <visp3/visual_features/vpFeaturePoint.h> 83 #include <visp3/vs/vpServo.h> 86 #define GETOPTARGS "cdh" 88 void usage(
const char *name,
const char *badparam);
89 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
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)
146 click_allowed =
false;
152 usage(argv[0], NULL);
157 usage(argv[0], optarg_);
163 if ((c == 1) || (c == -1)) {
165 usage(argv[0], NULL);
166 std::cerr <<
"ERROR: " << std::endl;
167 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
174 int main(
int argc,
const char **argv)
176 std::string appveyor_threading =
"";
182 if (appveyor_threading ==
"true") {
184 bool opt_click_allowed =
true;
185 bool opt_display =
true;
188 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
194 #if defined VISP_HAVE_X11 196 #elif defined VISP_HAVE_GDI 198 #elif defined VISP_HAVE_OPENCV 206 displayInt.
init(Iint, 700, 0,
"Internal view");
211 std::cout << std::endl;
212 std::cout <<
"----------------------------------------------" << std::endl;
213 std::cout <<
" Test program for vpServo " << std::endl;
214 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
215 std::cout <<
" Simulation " << std::endl;
216 std::cout <<
" task : servo 4 points " << std::endl;
217 std::cout <<
"----------------------------------------------" << std::endl;
218 std::cout << std::endl;
232 for (
unsigned int i = 0; i < 4; i++)
237 for (
unsigned int i = 0; i < 4; i++)
247 for (
unsigned int i = 0; i < 4; i++)
248 point[i].track(cdMo);
250 for (
unsigned int i = 0; i < 4; i++)
260 for (
unsigned int i = 0; i < 4; i++)
278 robot.initialiseObjectRelativeToCamera(cMo);
281 robot.setDesiredCameraPosition(cdMo);
285 robot.getCameraParameters(cam, Iint);
290 robot.getInternalView(Iint);
297 unsigned int iter = 0;
299 while (iter++ < 500) {
300 std::cout <<
"---------------------------------------------" << iter << std::endl;
307 cMo = robot.get_cMo();
310 std::cout <<
"Initial robot position with respect to the object frame:\n";
315 for (
unsigned int i = 0; i < 4; i++) {
324 robot.getInternalView(Iint);
328 if (opt_display && opt_click_allowed && iter == 1) {
330 std::cout <<
"Click in the internal view window to continue..." << std::endl;
340 std::cout <<
"|| s - s* || " << (task.
getError()).sumSquare() << std::endl;
350 std::cout <<
"Final robot position with respect to the object frame:\n";
353 if (opt_display && opt_click_allowed) {
355 std::cout <<
"Click in the internal view window to end..." << std::endl;
360 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
369 vpERROR_TRACE(
"You do not have X11, OpenCV or GDI display functionalities " 370 "or threading capabilities...");
VISP_EXPORT int wait(double t0, double t)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
void print() const
Print the matrix as a pose vector .
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 what is a point.
Initialize the velocity controller.
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)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Implementation of column vector and the associated operations.
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpColVector getError() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)