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;
74 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \ 75 !defined(VISP_HAVE_GTK) 78 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
79 std::cout <<
"You should install one of the following third-party library: " 80 "X11, OpenCV, GDI, GTK." 96 void execute(
unsigned int nbIter);
99 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C);
115 std::cout <<
"Catch an exception: " << e << std::endl;
121 #if defined VISP_HAVE_X11 123 #elif defined VISP_HAVE_OPENCV 125 #elif defined VISP_HAVE_GDI 127 #elif defined VISP_HAVE_D3D9 129 #elif defined VISP_HAVE_GTK 160 vector<vpPoint> src_pts;
161 vector<vpPoint> dst_pts;
163 double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
164 double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
167 for (
int i = 0; i < nbpoints; i++) {
170 src_pts.push_back(p);
175 for (
int i = 0; i < nbpoints; i++) {
178 dst_pts.push_back(p);
186 double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
187 double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
189 vector<vpPoint> cur_pts;
191 for (
int i = 0; i < nbpoints; i++) {
194 cur_pts.push_back(p);
204 displayInt.
init(Iint, 700, 0,
"Visual servoing with moments");
228 planeToABC(pl, A, B, C);
232 planeToABC(pl, Ad, Bd, Cd);
254 featureMomentsDes->
updateAll(Ad, Bd, Cd);
264 (1 << 10) | (1 << 11));
270 void execute(
unsigned int nbIter)
277 vpTRACE(
"Display task information ");
281 robot.getInternalView(Iint);
283 unsigned int iter = 0;
286 while (iter++ < nbIter) {
290 cMo = robot.get_cMo();
296 planeToABC(pl, A, B, C);
307 robot.getInternalView(Iint);
319 _error = (task.
getError()).sumSquare();
324 vpTRACE(
"\n\nClick in the internal view window to end...");
329 delete featureMoments;
330 delete featureMomentsDes;
334 double error() {
return _error; }
357 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
359 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
360 std::cout <<
"Invalid position:" << std::endl;
361 std::cout << cMo << std::endl;
362 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
374 robot.setCurrentViewColor(
vpColor(150, 150, 150));
375 robot.setDesiredViewColor(
vpColor(200, 200, 200));
377 removeJointLimits(robot);
381 robot.initialiseObjectRelativeToCamera(cMo);
384 robot.setDesiredCameraPosition(cdMo);
385 robot.getCameraParameters(cam, Iint);
void setPosition(const vpHomogeneousMatrix &wMc)
VISP_EXPORT int wait(double t0, double t)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Display for windows using GDI (available on any windows 32 platform).
Class to define colors available for display functionnalities.
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)
This class allows to access common vpFeatureMoments in a pre-filled database.
Initialize the position controller.
error that can be emited by ViSP classes.
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void setABCD(const double a, const double b, const double c, const double d)
Class for generic objects.
void extract(vpRotationMatrix &R) const
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
vpColVector computeControlLaw()
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void changeFrame(const vpHomogeneousMatrix &cMo)
static std::vector< double > getMu3(vpMomentObject &object)
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.
Class which enables to project an image in the 3D space and get the view of a virtual camera...
Simulator of Irisa's gantry robot named Afma6.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
vpServoIteractionMatrixType
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void updateAll(vpMomentObject &object)
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
Implementation of column vector and the associated operations.
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void setType(vpObjectType input_type)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpColVector getError() const
This class defines the container for a plane geometrical structure.
vpFeatureMomentAreaNormalized & getFeatureAn()
void setServo(const vpServoType &servo_type)
Class that consider the case of a translation vector.