 |
Visual Servoing Platform
version 3.3.0
|
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;
76 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
77 !defined(VISP_HAVE_GTK)
80 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
81 std::cout <<
"You should install one of the following third-party library: "
82 "X11, OpenCV, GDI, GTK."
99 void execute(
unsigned int nbIter);
102 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C);
118 std::cout <<
"Catch an exception: " << e << std::endl;
124 #if defined VISP_HAVE_X11
126 #elif defined VISP_HAVE_OPENCV
128 #elif defined VISP_HAVE_GDI
130 #elif defined VISP_HAVE_D3D9
132 #elif defined VISP_HAVE_GTK
162 std::vector<vpPoint> src_pts;
163 std::vector<vpPoint> dst_pts;
165 double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
166 double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
169 for (
int i = 0; i < nbpoints; i++) {
170 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
172 src_pts.push_back(p);
177 for (
int i = 0; i < nbpoints; i++) {
178 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
180 dst_pts.push_back(p);
201 planeToABC(pl, A, B, C);
205 planeToABC(pl, Ad, Bd, Cd);
227 featureMomentsDes->
updateAll(Ad, Bd, Cd);
236 (1 << 3) | (1 << 5));
246 double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
247 double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
249 std::vector<vpPoint> cur_pts;
251 for (
int i = 0; i < nbpoints; i++) {
252 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
254 cur_pts.push_back(p);
266 displayInt.
init(Iint, 700, 0,
"Visual servoing with moments");
275 void execute(
unsigned int nbIter)
282 vpTRACE(
"Display task information ");
286 robot.getInternalView(Iint);
288 unsigned int iter = 0;
291 while (iter++ < nbIter) {
294 cMo = robot.get_cMo();
300 planeToABC(pl, A, B, C);
311 robot.getInternalView(Iint);
322 _error = (task.
getError()).sumSquare();
327 vpTRACE(
"\n\nClick in the internal view window to end...");
332 delete featureMoments;
333 delete featureMomentsDes;
359 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
361 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
362 std::cout <<
"Invalid position:" << std::endl;
363 std::cout << cMo << std::endl;
364 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
376 robot.setCurrentViewColor(
vpColor(150, 150, 150));
377 robot.setDesiredViewColor(
vpColor(200, 200, 200));
379 removeJointLimits(robot);
381 robot.setConstantSamplingTimeMode(
true);
384 robot.initialiseObjectRelativeToCamera(cMo);
387 robot.setDesiredCameraPosition(cdMo);
388 robot.getCameraParameters(cam, Iint);
392 double error() {
return _error; }
Initialize the velocity controller.
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
This class allows to access common vpFeatureMoments in a pre-filled database.
void setType(vpObjectType input_type)
static double rad(double deg)
Generic class defining intrinsic camera parameters.
void updateAll(vpMomentObject &object)
vpFeatureMomentAlpha & getFeatureAlpha()
This class initializes and allows access to commonly used moments.
void setMaxTranslationVelocity(double maxVt)
Display for windows using GDI (available on any windows 32 platform).
Class that consider the case of a translation vector.
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Implementation of column vector and the associated operations.
void extract(vpRotationMatrix &R) const
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
void setServo(const vpServoType &servo_type)
void setABCD(double a, double b, double c, double d)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Simulator of Irisa's gantry robot named Afma6.
static void display(const vpImage< unsigned char > &I)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void fromVector(std::vector< vpPoint > &points)
void setMaxRotationVelocity(double maxVr)
vpColVector getError() const
void updateAll(double A, double B, double C)
static std::vector< double > getMu3(vpMomentObject &object)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Class for generic objects.
This class defines the container for a plane geometrical structure.
void changeFrame(const vpHomogeneousMatrix &cMo)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpServoIteractionMatrixType
vpColVector computeControlLaw()
static void flush(const vpImage< unsigned char > &I)
Class that defines what is a point.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Class to define colors available for display functionnalities.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double getSurface(vpMomentObject &object)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
vpFeatureMomentCInvariant & getFeatureCInvariant()
error that can be emited by ViSP classes.
static double getAlpha(vpMomentObject &object)
vpFeatureMomentAreaNormalized & getFeatureAn()