46 #include <visp3/core/vpCameraParameters.h> 47 #include <visp3/core/vpConfig.h> 48 #include <visp3/core/vpDebug.h> 49 #include <visp3/core/vpHomogeneousMatrix.h> 50 #include <visp3/core/vpIoTools.h> 51 #include <visp3/core/vpMath.h> 52 #include <visp3/core/vpMomentCommon.h> 53 #include <visp3/core/vpMomentDatabase.h> 54 #include <visp3/core/vpMomentObject.h> 55 #include <visp3/core/vpPlane.h> 56 #include <visp3/gui/vpDisplayGDI.h> 57 #include <visp3/gui/vpDisplayGTK.h> 58 #include <visp3/gui/vpDisplayX.h> 59 #include <visp3/robot/vpSimulatorAfma6.h> 60 #include <visp3/visual_features/vpFeatureBuilder.h> 61 #include <visp3/visual_features/vpFeatureMomentCommon.h> 62 #include <visp3/visual_features/vpFeaturePoint.h> 63 #include <visp3/vs/vpServo.h> 65 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD) 69 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is " 72 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." 97 void execute(
unsigned int nbIter);
100 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C);
116 std::cout <<
"Catch an exception: " << e << std::endl;
122 #if defined VISP_HAVE_X11 124 #elif defined VISP_HAVE_OPENCV 126 #elif defined VISP_HAVE_GDI 128 #elif defined VISP_HAVE_D3D9 130 #elif defined VISP_HAVE_GTK 160 std::vector<vpPoint> src_pts;
161 std::vector<vpPoint> dst_pts;
163 double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
164 double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
167 for (
int i = 0; i < nbpoints; i++) {
168 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
170 src_pts.push_back(p);
175 for (
int i = 0; i < nbpoints; i++) {
176 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
178 dst_pts.push_back(p);
199 planeToABC(pl, A, B, C);
203 planeToABC(pl, Ad, Bd, Cd);
225 featureMomentsDes->
updateAll(Ad, Bd, Cd);
234 (1 << 3) | (1 << 5));
244 double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
245 double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
247 std::vector<vpPoint> cur_pts;
249 for (
int i = 0; i < nbpoints; i++) {
250 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
252 cur_pts.push_back(p);
264 displayInt.
init(Iint, 700, 0,
"Visual servoing with moments");
273 void execute(
unsigned int nbIter)
280 vpTRACE(
"Display task information ");
284 robot.getInternalView(Iint);
286 unsigned int iter = 0;
289 while (iter++ < nbIter) {
292 cMo = robot.get_cMo();
298 planeToABC(pl, A, B, C);
309 robot.getInternalView(Iint);
320 _error = (task.
getError()).sumSquare();
325 vpTRACE(
"\n\nClick in the internal view window to end...");
330 delete featureMoments;
331 delete featureMomentsDes;
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);
379 robot.setConstantSamplingTimeMode(
true);
382 robot.initialiseObjectRelativeToCamera(cMo);
385 robot.setDesiredCameraPosition(cdMo);
386 robot.getCameraParameters(cam, Iint);
390 double error() {
return _error; }
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setMaxTranslationVelocity(const double maxVt)
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.
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)
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...
Initialize the velocity controller.
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)
void setMaxRotationVelocity(const double maxVr)
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.