44 #include <visp3/core/vpDebug.h>
45 #include <visp3/core/vpConfig.h>
47 #include <visp3/core/vpHomogeneousMatrix.h>
48 #include <visp3/core/vpMomentObject.h>
49 #include <visp3/core/vpMomentDatabase.h>
50 #include <visp3/core/vpMomentCommon.h>
51 #include <visp3/visual_features/vpFeatureMomentCommon.h>
52 #include <visp3/gui/vpDisplayX.h>
53 #include <visp3/gui/vpDisplayGTK.h>
54 #include <visp3/gui/vpDisplayGDI.h>
55 #include <visp3/core/vpCameraParameters.h>
56 #include <visp3/core/vpIoTools.h>
57 #include <visp3/core/vpMath.h>
58 #include <visp3/core/vpHomogeneousMatrix.h>
59 #include <visp3/vs/vpServo.h>
60 #include <visp3/core/vpDebug.h>
61 #include <visp3/visual_features/vpFeatureBuilder.h>
62 #include <visp3/visual_features/vpFeaturePoint.h>
63 #include <visp3/robot/vpSimulatorAfma6.h>
64 #include <visp3/core/vpPlane.h>
67 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
71 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
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) && !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: X11, OpenCV, GDI, GTK." << std::endl;
94 void execute(
unsigned int nbIter);
97 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
114 std::cout <<
"Catch an exception: " << e << std::endl;
120 #if defined VISP_HAVE_X11
122 #elif defined VISP_HAVE_OPENCV
124 #elif defined VISP_HAVE_GDI
126 #elif defined VISP_HAVE_D3D9
128 #elif defined VISP_HAVE_GTK
156 std::vector<vpPoint> src_pts;
157 std::vector<vpPoint> dst_pts;
159 double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
160 double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
163 for (
int i = 0 ; i < nbpoints ; i++){
164 vpPoint p(x[i]/20,y[i]/20,0.0);
166 src_pts.push_back(p);
171 for (
int i = 0 ; i < nbpoints ; i++){
172 vpPoint p(x[i]/20,y[i]/20,0.0);
174 dst_pts.push_back(p);
183 double A;
double B;
double C;
184 double Ad;
double Bd;
double Cd;
191 planeToABC(pl,A,B,C);
195 planeToABC(pl,Ad,Bd,Cd);
230 double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
231 double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
233 std::vector<vpPoint> cur_pts;
235 for (
int i = 0 ; i < nbpoints ; i++){
236 vpPoint p(x[i]/20,y[i]/20,0.0);
238 cur_pts.push_back(p);
250 displayInt.
init(Iint,700,0,
"Visual servoing with moments") ;
259 void execute(
unsigned int nbIter){
265 vpTRACE(
"Display task information " ) ;
269 robot.getInternalView(Iint);
274 while(iter++<nbIter ){
277 cMo = robot.get_cMo();
283 planeToABC(pl,A,B,C);
293 robot.getInternalView(Iint);
305 _error = ( task.
getError() ).sumSquare();
310 vpTRACE(
"\n\nClick in the internal view window to end...");
315 delete featureMoments;
316 delete featureMomentsDes;
342 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
343 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
344 std::cout <<
"Invalid position:" << std::endl;
345 std::cout << cMo << std::endl;
346 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
357 robot.setCurrentViewColor(
vpColor(150,150,150));
358 robot.setDesiredViewColor(
vpColor(200,200,200));
360 removeJointLimits(robot);
362 robot.setConstantSamplingTimeMode(
true);
364 robot.initialiseObjectRelativeToCamera(cMo);
367 robot.setDesiredCameraPosition(cdMo);
368 robot.getCameraParameters(cam,Iint);
372 double error(){
return _error;}
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Perspective projection without distortion model.
void setMaxTranslationVelocity(const double maxVt)
Implementation of an homogeneous matrix and operations on such kind of matrices.
A plate with 8 points at coordinates (0.05,0,0), (0.15,0.05,0), (0.2,0.2,0), (-0.05,0.2,0), (-0.15,-0.1,0), (-0.1,-0.1,0), (-0.05,0.05,0) and (0.5,0,0). ach point is represented by a circle with 2cm radius.
Display for windows using GDI (available on any windows 32 platform).
Class to define colors available for display functionnalities.
Define the X11 console to display images.
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 init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
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.
Initialize the velocity controller.
vpColVector getError() const
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.
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+ library version 1.2.
void extract(vpRotationMatrix &R) const
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 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)
virtual bool getClick(bool blocking=true)=0
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.