54 #include <visp3/core/vpConfig.h>
56 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
57 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
62 #include <visp3/core/vpCameraParameters.h>
63 #include <visp3/core/vpHomogeneousMatrix.h>
64 #include <visp3/core/vpImage.h>
65 #include <visp3/core/vpMath.h>
66 #include <visp3/gui/vpDisplayGDI.h>
67 #include <visp3/gui/vpDisplayGTK.h>
68 #include <visp3/gui/vpDisplayOpenCV.h>
69 #include <visp3/gui/vpDisplayX.h>
70 #include <visp3/gui/vpProjectionDisplay.h>
71 #include <visp3/io/vpParseArgv.h>
72 #include <visp3/robot/vpSimulatorCamera.h>
73 #include <visp3/visual_features/vpFeatureBuilder.h>
74 #include <visp3/visual_features/vpFeaturePoint.h>
75 #include <visp3/vs/vpServo.h>
76 #include <visp3/vs/vpServoDisplay.h>
79 #define GETOPTARGS "cdh"
81 void usage(
const char *name,
const char *badparam);
82 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
92 void usage(
const char *name,
const char *badparam)
95 Tests a control law with the following characteristics:\n\
96 - eye-in-hand control\n\
97 - articular velocity are computed\n\
98 - servo on 4 points,\n\
99 - internal and external camera view displays.\n\
102 %s [-c] [-d] [-h]\n",
108 Disable the mouse click. Useful to automate the \n\
109 execution of this program without human 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],
nullptr);
150 usage(argv[0], optarg_);
155 if ((c == 1) || (c == -1)) {
157 usage(argv[0],
nullptr);
158 std::cerr <<
"ERROR: " << std::endl;
159 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
166 int main(
int argc,
const char **argv)
169 bool opt_click_allowed =
true;
170 bool opt_display =
true;
173 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
179 #if defined(VISP_HAVE_X11)
182 #elif defined(VISP_HAVE_GTK)
185 #elif defined(VISP_HAVE_GDI)
188 #elif defined(HAVE_OPENCV_HIGHGUI)
199 displayInt.
init(Iint, 0, 0,
"Internal view");
200 displayExt.
init(Iext, 330, 000,
"External view");
204 double px = 500, py = 500;
205 double u0 = 150, v0 = 160;
212 std::cout << std::endl;
213 std::cout <<
"----------------------------------------------" << std::endl;
214 std::cout <<
" Test program for vpServo " << std::endl;
215 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
216 std::cout <<
" Simulation " << std::endl;
217 std::cout <<
" task : servo 4 points " << std::endl;
218 std::cout <<
"----------------------------------------------" << std::endl;
219 std::cout << std::endl;
226 robot.getPosition(wMc);
238 for (
unsigned i = 0; i < 4; i++)
239 externalview.
insert(point[i]);
243 for (
unsigned i = 0; i < 4; i++)
248 for (
unsigned i = 0; i < 4; i++)
276 for (
unsigned i = 0; i < 4; i++)
285 unsigned int iter = 0;
287 while (iter++ < 200) {
288 std::cout <<
"---------------------------------------------" << iter << std::endl;
297 robot.getPosition(wMc);
302 for (
unsigned i = 0; i < 4; i++) {
329 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
335 std::cout <<
"Final robot position with respect to the object frame:\n";
338 if (opt_display && opt_click_allowed) {
345 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
349 #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
352 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
358 std::cout <<
"You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..."
360 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
361 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
362 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
363 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
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
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)
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 vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
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 ...
void setWorldCoordinates(double oX, double oY, double oZ)
interface with the image for feature display
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 insert(vpForwardProjection &fp)
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()
Class that defines the simplest robot: a free flying camera.
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.