 |
Visual Servoing Platform
version 3.3.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...
@ divideByZeroError
Division by zero.
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.
@ perspectiveProjWithoutDistortion
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)
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)
void setABCD(double a, double b, double c, double d)
VISP_EXPORT double measureTimeMs()
@ STATE_POSITION_CONTROL
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 addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
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()
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()