53 #include <visp3/core/vpConfig.h>
54 #include <visp3/core/vpDebug.h>
56 #if defined(VISP_HAVE_DISPLAY)
61 #include <visp3/core/vpCameraParameters.h>
62 #include <visp3/core/vpHomogeneousMatrix.h>
63 #include <visp3/core/vpImage.h>
64 #include <visp3/core/vpImagePoint.h>
65 #include <visp3/core/vpIoTools.h>
66 #include <visp3/core/vpMath.h>
67 #include <visp3/core/vpMeterPixelConversion.h>
68 #include <visp3/gui/vpDisplayFactory.h>
69 #include <visp3/gui/vpProjectionDisplay.h>
70 #include <visp3/io/vpParseArgv.h>
71 #include <visp3/robot/vpSimulatorCamera.h>
72 #include <visp3/visual_features/vpFeatureBuilder.h>
73 #include <visp3/visual_features/vpFeaturePointPolar.h>
74 #include <visp3/vs/vpServo.h>
75 #include <visp3/vs/vpServoDisplay.h>
78 #define GETOPTARGS "cdh"
80 #ifdef ENABLE_VISP_NAMESPACE
84 void usage(
const char *name,
const char *badparam);
85 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
95 void usage(
const char *name,
const char *badparam)
98 Tests a control law with the following characteristics:\n\
99 - eye-in-hand control\n\
100 - articular velocity are computed\n\
101 - servo on 4 points,\n\
102 - internal and external camera view displays.\n\
105 %s [-c] [-d] [-h]\n",
111 Disable the mouse click. Useful to automate the \n\
112 execution of this program without human intervention.\n\
115 Turn off the display.\n\
121 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
135 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
143 click_allowed =
false;
149 usage(argv[0],
nullptr);
153 usage(argv[0], optarg_);
158 if ((c == 1) || (c == -1)) {
160 usage(argv[0],
nullptr);
161 std::cerr <<
"ERROR: " << std::endl;
162 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
169 int main(
int argc,
const char **argv)
172 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
173 std::shared_ptr<vpDisplay> displayInt;
174 std::shared_ptr<vpDisplay> displayExt;
186 std::string username;
191 std::string logdirname;
193 logdirname =
"C:/temp/" + username;
195 logdirname =
"/tmp/" + username;
205 std::cerr << std::endl <<
"ERROR:" << std::endl;
206 std::cerr <<
" Cannot create " << logdirname << std::endl;
210 std::string logfilename;
211 logfilename = logdirname +
"/log.dat";
214 std::ofstream flog(logfilename.c_str());
216 bool opt_click_allowed =
true;
217 bool opt_display =
true;
220 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
232 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
242 double px = 500, py = 500;
243 double u0 = 150, v0 = 160;
250 std::cout << std::endl;
251 std::cout <<
"----------------------------------------------" << std::endl;
252 std::cout <<
" Test program for vpServo " << std::endl;
253 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
254 std::cout <<
" Simulation " << std::endl;
255 std::cout <<
" task : servo 4 points " << std::endl;
256 std::cout <<
"----------------------------------------------" << std::endl;
257 std::cout << std::endl;
266 #if defined(TRANS_Z_PURE)
271 #elif defined(TRANS_X_PURE)
277 #elif defined(ROT_Z_PURE)
283 #elif defined(ROT_X_PURE)
289 #elif defined(COMPLEX)
295 #elif defined(PROBLEM)
305 robot.getPosition(wMc);
317 for (
unsigned int i = 0; i < 4; i++)
318 externalview.
insert(point[i]);
325 for (
unsigned int i = 0; i < 4; i++) {
326 point[i].
track(cMod);
334 for (
unsigned int i = 0; i < 4; i++)
339 for (
unsigned int i = 0; i < 4; i++) {
364 for (
unsigned int i = 0; i < 4; i++)
370 std::cout <<
"\nDisplay task information: " << std::endl;
373 unsigned int iter = 0;
375 while (iter++ < 200) {
376 std::cout <<
"---------------------------------------------" << iter << std::endl;
385 robot.getPosition(wMc);
390 for (
unsigned int i = 0; i < 4; i++) {
410 std::cout <<
"Display task information: " << std::endl;
422 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" " << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
424 std::cout <<
"v: " << v.
t() << std::endl;
426 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
431 flog << (task.
getError()).t() <<
" ";
432 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
435 for (
unsigned int i = 0; i < 4; i++) {
439 for (
unsigned int i = 0; i < 4; i++) {
440 flog << point[i].
get_x() <<
" " << point[i].
get_y() <<
" ";
449 std::cout <<
"\nClick in the internal camera view to continue..." << std::endl;
463 std::cout <<
"Final robot position with respect to the object frame:\n";
466 if (opt_display && opt_click_allowed) {
471 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
472 if (displayInt !=
nullptr) {
475 if (displayExt !=
nullptr) {
482 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
483 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
484 if (displayInt !=
nullptr) {
487 if (displayExt !=
nullptr) {
497 std::cout <<
"You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..."
499 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
500 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
501 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
502 std::cout <<
"- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor white
static const vpColor green
Class that defines generic functionalities for display.
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)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
Class that defines 2D image point visual feature with polar coordinates described in .
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double rad(double deg)
Implementation of a matrix and operations on matrices.
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 ...
double get_y() const
Get the point y coordinate in the image plane.
double get_x() const
Get the point x coordinate in the image plane.
void setWorldCoordinates(double oX, double oY, double oZ)
interface with the image for feature display
void insert(vpForwardProjection &fp)
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
void get_eJe(vpMatrix &eJe) VP_OVERRIDE
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
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 set_cVe(const vpVelocityTwistMatrix &cVe_)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
@ FEATURE_CURRENT
Print the current features .
@ FEATURE_DESIRED
Print the desired features .
Class that defines the simplest robot: a free flying camera.
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.