48 #include <visp/vpDebug.h>
49 #include <visp/vpConfig.h>
51 #include <visp/vpHomogeneousMatrix.h>
52 #include <visp/vpMomentObject.h>
53 #include <visp/vpMomentDatabase.h>
54 #include <visp/vpMomentCommon.h>
55 #include <visp/vpFeatureMomentCommon.h>
56 #include <visp/vpDisplayX.h>
57 #include <visp/vpDisplayGTK.h>
58 #include <visp/vpDisplayGDI.h>
59 #include <visp/vpCameraParameters.h>
60 #include <visp/vpIoTools.h>
61 #include <visp/vpMath.h>
62 #include <visp/vpHomogeneousMatrix.h>
63 #include <visp/vpServo.h>
64 #include <visp/vpDebug.h>
65 #include <visp/vpFeatureBuilder.h>
66 #include <visp/vpFeaturePoint.h>
67 #include <visp/vpSimulatorAfma6.h>
68 #include <visp/vpPlane.h>
81 void execute(
unsigned int nbIter);
84 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
87 #if !defined(WIN32) && !defined(VISP_HAVE_PTHREAD)
91 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
92 std::cout <<
"You should install pthread third-party library." << std::endl;
95 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
98 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
99 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
115 #if defined VISP_HAVE_X11
117 #elif defined VISP_HAVE_OPENCV
119 #elif defined VISP_HAVE_GDI
121 #elif defined VISP_HAVE_D3D9
123 #elif defined VISP_HAVE_GTK
153 vector<vpPoint> src_pts;
154 vector<vpPoint> dst_pts;
156 double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
157 double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
160 for (
int i = 0 ; i < nbpoints ; i++){
164 src_pts.push_back(p);
169 for (
int i = 0 ; i < nbpoints ; i++){
173 dst_pts.push_back(p);
182 double A;
double B;
double C;
183 double Ad;
double Bd;
double Cd;
190 _planeToABC(pl,A,B,C);
194 _planeToABC(pl,Ad,Bd,Cd);
229 double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
230 double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
232 vector<vpPoint> cur_pts;
234 for (
int i = 0 ; i < nbpoints ; i++){
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;
341 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
342 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
343 std::cout <<
"Invalid position:" << std::endl;
344 std::cout << cMo << std::endl;
345 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
359 removeJointLimits(robot);
371 double error(){
return _error;}
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
Perspective projection without distortion model.
void setMaxTranslationVelocity(const double maxVt)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
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.
static double getAlpha(vpMomentObject &objec)
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)
create a new ste of two visual features
This class allows to access common vpFeatureMoments in a pre-filled database.
void setLambda(double _lambda)
set the gain lambda
error that can be emited by ViSP classes.
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
Class for generic objects.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void track(const vpHomogeneousMatrix &cMo)
void setType(vpObjectType type)
void setABCD(const double A, const double B, const double C, const double D)
static void flush(const vpImage< unsigned char > &I)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
Display for windows using Direct3D.
void kill()
destruction (memory deallocation if required)
Initialize the velocity controller.
vpColVector getError() const
vpColVector computeControlLaw()
compute the desired control law
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
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
void init(vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion)
vpServoIteractionMatrixType
void setCurrentViewColor(const vpColor col)
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
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.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void setDesiredViewColor(const vpColor col)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
virtual bool getClick(bool blocking=true)=0
This class defines the container for a plane geometrical structure.
Class required to compute the visual servoing control law descbribed in and .
vpFeatureMomentAreaNormalized & getFeatureAn()
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
Class that consider the case of a translation vector.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...