 |
Visual Servoing Platform
version 3.2.0
|
45 #include <visp3/core/vpCameraParameters.h>
46 #include <visp3/core/vpConfig.h>
47 #include <visp3/core/vpDebug.h>
48 #include <visp3/core/vpHomogeneousMatrix.h>
49 #include <visp3/core/vpIoTools.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpMomentCommon.h>
52 #include <visp3/core/vpMomentDatabase.h>
53 #include <visp3/core/vpMomentObject.h>
54 #include <visp3/core/vpPlane.h>
55 #include <visp3/gui/vpDisplayGDI.h>
56 #include <visp3/gui/vpDisplayGTK.h>
57 #include <visp3/gui/vpDisplayX.h>
58 #include <visp3/robot/vpSimulatorAfma6.h>
59 #include <visp3/visual_features/vpFeatureBuilder.h>
60 #include <visp3/visual_features/vpFeatureMomentCommon.h>
61 #include <visp3/visual_features/vpFeaturePoint.h>
62 #include <visp3/vs/vpServo.h>
64 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
68 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is "
71 std::cout <<
"You should install pthread third-party library." << std::endl;
75 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
76 !defined(VISP_HAVE_GTK)
79 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
80 std::cout <<
"You should install one of the following third-party library: "
81 "X11, OpenCV, GDI, GTK."
98 void execute(
unsigned int nbIter);
101 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C);
117 std::cout <<
"Catch an exception: " << e << std::endl;
123 #if defined VISP_HAVE_X11
125 #elif defined VISP_HAVE_OPENCV
127 #elif defined VISP_HAVE_GDI
129 #elif defined VISP_HAVE_D3D9
131 #elif defined VISP_HAVE_GTK
162 vector<vpPoint> src_pts;
163 vector<vpPoint> dst_pts;
165 double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
166 double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
169 for (
int i = 0; i < nbpoints; i++) {
172 src_pts.push_back(p);
177 for (
int i = 0; i < nbpoints; i++) {
180 dst_pts.push_back(p);
188 double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
189 double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
191 vector<vpPoint> cur_pts;
193 for (
int i = 0; i < nbpoints; i++) {
196 cur_pts.push_back(p);
206 displayInt.
init(Iint, 700, 0,
"Visual servoing with moments");
230 planeToABC(pl, A, B, C);
234 planeToABC(pl, Ad, Bd, Cd);
256 featureMomentsDes->
updateAll(Ad, Bd, Cd);
266 (1 << 10) | (1 << 11));
272 void execute(
unsigned int nbIter)
279 vpTRACE(
"Display task information ");
283 robot.getInternalView(Iint);
285 unsigned int iter = 0;
288 while (iter++ < nbIter) {
292 cMo = robot.get_cMo();
298 planeToABC(pl, A, B, C);
309 robot.getInternalView(Iint);
321 _error = (task.
getError()).sumSquare();
326 vpTRACE(
"\n\nClick in the internal view window to end...");
331 delete featureMoments;
332 delete featureMomentsDes;
336 double error() {
return _error; }
359 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
361 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
362 std::cout <<
"Invalid position:" << std::endl;
363 std::cout << cMo << std::endl;
364 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
376 robot.setCurrentViewColor(
vpColor(150, 150, 150));
377 robot.setDesiredViewColor(
vpColor(200, 200, 200));
379 removeJointLimits(robot);
383 robot.initialiseObjectRelativeToCamera(cMo);
386 robot.setDesiredCameraPosition(cdMo);
387 robot.getCameraParameters(cam, Iint);
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
This class allows to access common vpFeatureMoments in a pre-filled database.
VISP_EXPORT int wait(double t0, double t)
void setType(vpObjectType input_type)
static double rad(double deg)
Generic class defining intrinsic camera parameters.
void updateAll(vpMomentObject &object)
vpFeatureMomentAlpha & getFeatureAlpha()
This class initializes and allows access to commonly used moments.
void setPosition(const vpHomogeneousMatrix &wMc)
Display for windows using GDI (available on any windows 32 platform).
Class that consider the case of a translation vector.
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void setABCD(const double a, const double b, const double c, const double d)
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Implementation of column vector and the associated operations.
void extract(vpRotationMatrix &R) const
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
void setServo(const vpServoType &servo_type)
VISP_EXPORT double measureTimeMs()
Initialize the position controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Simulator of Irisa's gantry robot named Afma6.
static void display(const vpImage< unsigned char > &I)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void fromVector(std::vector< vpPoint > &points)
vpColVector getError() const
void updateAll(double A, double B, double C)
static std::vector< double > getMu3(vpMomentObject &object)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Class for generic objects.
This class defines the container for a plane geometrical structure.
void changeFrame(const vpHomogeneousMatrix &cMo)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpServoIteractionMatrixType
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(const vpImage< unsigned char > &I)
Class that defines what is a point.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Class to define colors available for display functionnalities.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double getSurface(vpMomentObject &object)
vpFeatureMomentCInvariant & getFeatureCInvariant()
error that can be emited by ViSP classes.
static double getAlpha(vpMomentObject &object)
vpFeatureMomentAreaNormalized & getFeatureAn()