43 #include <visp3/core/vpDebug.h> 44 #include <visp3/core/vpConfig.h> 46 #include <visp3/core/vpHomogeneousMatrix.h> 47 #include <visp3/core/vpMomentObject.h> 48 #include <visp3/core/vpMomentDatabase.h> 49 #include <visp3/core/vpMomentCommon.h> 50 #include <visp3/visual_features/vpFeatureMomentCommon.h> 51 #include <visp3/gui/vpDisplayX.h> 52 #include <visp3/gui/vpDisplayGTK.h> 53 #include <visp3/gui/vpDisplayGDI.h> 54 #include <visp3/core/vpCameraParameters.h> 55 #include <visp3/core/vpIoTools.h> 56 #include <visp3/core/vpMath.h> 57 #include <visp3/core/vpHomogeneousMatrix.h> 58 #include <visp3/vs/vpServo.h> 59 #include <visp3/core/vpDebug.h> 60 #include <visp3/visual_features/vpFeatureBuilder.h> 61 #include <visp3/visual_features/vpFeaturePoint.h> 62 #include <visp3/robot/vpSimulatorAfma6.h> 63 #include <visp3/core/vpPlane.h> 65 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD) 69 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
70 std::cout <<
"You should install pthread third-party library." << std::endl;
73 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK) 76 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
77 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
92 void execute(
unsigned int nbIter);
95 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
112 std::cout <<
"Catch an exception: " << e << std::endl;
118 #if defined VISP_HAVE_X11 120 #elif defined VISP_HAVE_OPENCV 122 #elif defined VISP_HAVE_GDI 124 #elif defined VISP_HAVE_D3D9 126 #elif defined VISP_HAVE_GTK 155 vector<vpPoint> src_pts;
156 vector<vpPoint> dst_pts;
158 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
159 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
162 for (
int i = 0 ; i < nbpoints ; i++){
165 src_pts.push_back(p);
170 for (
int i = 0 ; i < nbpoints ; i++){
173 dst_pts.push_back(p);
181 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
182 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
184 vector<vpPoint> cur_pts;
186 for (
int i = 0 ; i < nbpoints ; i++){
189 cur_pts.push_back(p);
199 displayInt.
init(Iint,700,0,
"Visual servoing with moments") ;
210 double A;
double B;
double C;
211 double Ad;
double Bd;
double Cd;
218 planeToABC(pl,A,B,C);
222 planeToABC(pl,Ad,Bd,Cd);
254 void execute(
unsigned int nbIter){
260 vpTRACE(
"Display task information " ) ;
264 robot.getInternalView(Iint);
269 while(iter++<nbIter ){
273 cMo = robot.get_cMo();
279 planeToABC(pl,A,B,C);
289 robot.getInternalView(Iint);
300 _error = ( task.
getError() ).sumSquare();
305 vpTRACE(
"\n\nClick in the internal view window to end...");
310 delete featureMoments;
311 delete featureMomentsDes;
315 double error(){
return _error;}
338 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
339 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
340 std::cout <<
"Invalid position:" << std::endl;
341 std::cout << cMo << std::endl;
342 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
353 robot.setCurrentViewColor(
vpColor(150,150,150));
354 robot.setDesiredViewColor(
vpColor(200,200,200));
356 removeJointLimits(robot);
359 robot.initialiseObjectRelativeToCamera(cMo);
362 robot.setDesiredCameraPosition(cdMo);
363 robot.getCameraParameters(cam,Iint);
void setPosition(const vpHomogeneousMatrix &wMc)
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
VISP_EXPORT int wait(double t0, double t)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Perspective projection without distortion model.
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="")
A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0...
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.