56 #include <visp3/core/vpConfig.h> 58 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) 63 #include <visp3/core/vpCameraParameters.h> 64 #include <visp3/core/vpHomogeneousMatrix.h> 65 #include <visp3/core/vpImage.h> 66 #include <visp3/core/vpMath.h> 67 #include <visp3/gui/vpDisplayGDI.h> 68 #include <visp3/gui/vpDisplayGTK.h> 69 #include <visp3/gui/vpDisplayOpenCV.h> 70 #include <visp3/gui/vpDisplayX.h> 71 #include <visp3/gui/vpProjectionDisplay.h> 72 #include <visp3/io/vpParseArgv.h> 73 #include <visp3/robot/vpSimulatorCamera.h> 74 #include <visp3/visual_features/vpFeatureBuilder.h> 75 #include <visp3/visual_features/vpFeaturePoint.h> 76 #include <visp3/vs/vpServo.h> 77 #include <visp3/vs/vpServoDisplay.h> 80 #define GETOPTARGS "cdh" 82 void usage(
const char *name,
const char *badparam);
83 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
93 void usage(
const char *name,
const char *badparam)
96 Tests a control law with the following characteristics:\n\ 97 - eye-in-hand control\n\ 98 - articular velocity are computed\n\ 99 - servo on 4 points,\n\ 100 - internal and external camera view displays.\n\ 103 %s [-c] [-d] [-h]\n", name);
108 Disable the mouse click. Useful to automaze the \n\ 109 execution of this program without humain intervention.\n\ 112 Turn off the display.\n\ 118 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
132 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
140 click_allowed =
false;
146 usage(argv[0], NULL);
151 usage(argv[0], optarg_);
157 if ((c == 1) || (c == -1)) {
159 usage(argv[0], NULL);
160 std::cerr <<
"ERROR: " << std::endl;
161 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
168 int main(
int argc,
const char **argv)
171 bool opt_click_allowed =
true;
172 bool opt_display =
true;
175 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
181 #if defined VISP_HAVE_X11 184 #elif defined VISP_HAVE_GTK 187 #elif defined VISP_HAVE_GDI 190 #elif defined VISP_HAVE_OPENCV 201 displayInt.
init(Iint, 0, 0,
"Internal view");
202 displayExt.
init(Iext, 330, 000,
"External view");
217 std::cout << std::endl;
218 std::cout <<
"----------------------------------------------" << std::endl;
219 std::cout <<
" Test program for vpServo " << std::endl;
220 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
221 std::cout <<
" Simulation " << std::endl;
222 std::cout <<
" task : servo 4 points " << std::endl;
223 std::cout <<
"----------------------------------------------" << std::endl;
224 std::cout << std::endl;
244 for (i = 0; i < 4; i++)
245 externalview.
insert(point[i]);
249 for (i = 0; i < 4; i++)
254 for (i = 0; i < 4; i++)
282 for (i = 0; i < 4; i++)
291 unsigned int iter = 0;
293 while (iter++ < 200) {
294 std::cout <<
"---------------------------------------------" << iter << std::endl;
308 for (i = 0; i < 4; i++) {
335 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
342 std::cout <<
"Final robot position with respect to the object frame:\n";
345 if (opt_display && opt_click_allowed) {
347 std::cout <<
"\n\nClick in the internal view window to end..." << std::endl;
352 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
359 int main() { std::cout <<
"You do not have X11, GTK, GDI or OpenCV display functionalities..." << std::endl; }
Implementation of a matrix and operations on matrices.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, const unsigned int thickness=1)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
Display for windows using GDI (available on any windows 32 platform).
void set_eJe(const vpMatrix &eJe_)
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)
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)
vpHomogeneousMatrix inverse() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
vpHomogeneousMatrix getPosition() const
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a point.
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.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void insert(vpForwardProjection &fp)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
void buildFrom(const double x, const double y, const double Z)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Implementation of column vector and the associated operations.
void set_cVe(const vpVelocityTwistMatrix &cVe_)
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 get_eJe(vpMatrix &eJe)
interface with the image for feature display
void setServo(const vpServoType &servo_type)
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)