 |
Visual Servoing Platform
version 3.2.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 setMaxTranslationVelocity(const double maxVt)
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.
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)
void setABCD(const double a, const double b, const double c, const double d)
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)
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)
vpColVector getError() const
void updateAll(double A, double B, double C)
void setMaxRotationVelocity(const double maxVr)
static std::vector< double > getMu3(vpMomentObject &object)
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()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
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()