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/vpDisplayD3D.h> 57 #include <visp3/gui/vpDisplayGDI.h> 58 #include <visp3/gui/vpDisplayGTK.h> 59 #include <visp3/gui/vpDisplayOpenCV.h> 60 #include <visp3/gui/vpDisplayX.h> 61 #include <visp3/gui/vpPlot.h> 62 #include <visp3/robot/vpSimulatorAfma6.h> 63 #include <visp3/visual_features/vpFeatureBuilder.h> 64 #include <visp3/visual_features/vpFeatureMomentCommon.h> 65 #include <visp3/visual_features/vpFeaturePoint.h> 66 #include <visp3/vs/vpServo.h> 68 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD) 72 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is " 75 std::cout <<
"You should install pthread third-party library." << std::endl;
79 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \ 80 !defined(VISP_HAVE_GTK) 83 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
84 std::cout <<
"You should install one of the following third-party library: " 85 "X11, OpenCV, GDI, GTK." 91 #ifndef DOXYGEN_SHOULD_SKIP_THIS 96 : m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(false), m_Iint(m_height, m_width, 255), m_task(), m_cam(),
97 m_error(0), m_imsim(), m_interaction_type(), m_src(6), m_dst(6), m_moments(NULL), m_momentsDes(NULL),
98 m_featureMoments(NULL), m_featureMomentsDes(NULL), m_displayInt(NULL)
103 #ifdef VISP_HAVE_DISPLAY 110 delete m_featureMoments;
111 delete m_featureMomentsDes;
117 std::vector<vpPoint> src_pts;
118 std::vector<vpPoint> dst_pts;
120 double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
121 double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
124 for (
int i = 0; i < nbpoints; i++) {
125 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
127 src_pts.push_back(p);
131 m_src.fromVector(src_pts);
132 for (
int i = 0; i < nbpoints; i++) {
133 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
135 dst_pts.push_back(p);
138 m_dst.fromVector(dst_pts);
157 planeToABC(pl, A, B, C);
161 planeToABC(pl, Ad, Bd, Cd);
179 m_moments->updateAll(m_src);
180 m_momentsDes->updateAll(m_dst);
182 m_featureMoments->updateAll(A, B, C);
183 m_featureMomentsDes->updateAll(Ad, Bd, Cd);
186 m_task.setInteractionMatrixType(m_interaction_type);
189 m_task.addFeature(m_featureMoments->getFeatureGravityNormalized(),
190 m_featureMomentsDes->getFeatureGravityNormalized());
191 m_task.addFeature(m_featureMoments->getFeatureAn(), m_featureMomentsDes->getFeatureAn());
192 m_task.addFeature(m_featureMoments->getFeatureCInvariant(), m_featureMomentsDes->getFeatureCInvariant(),
193 (1 << 3) | (1 << 5));
194 m_task.addFeature(m_featureMoments->getFeatureAlpha(), m_featureMomentsDes->getFeatureAlpha());
196 m_task.setLambda(1.);
204 double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
205 double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
207 std::vector<vpPoint> cur_pts;
209 for (
int i = 0; i < nbpoints; i++) {
210 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
212 cur_pts.push_back(p);
224 #ifdef VISP_HAVE_DISPLAY 226 #if defined VISP_HAVE_X11 228 #elif defined VISP_HAVE_OPENCV 230 #elif defined VISP_HAVE_GDI 232 #elif defined VISP_HAVE_D3D9 234 #elif defined VISP_HAVE_GTK 237 m_displayInt->
init(m_Iint, 50, 50,
"Visual servoing with moments");
248 void execute(
unsigned int nbIter)
251 init_visp_plot(ViSP_plot);
258 std::cout <<
"Display task information " << std::endl;
262 m_robot.getInternalView(m_Iint);
264 unsigned int iter = 0;
267 while (iter++ < nbIter) {
270 m_cMo = m_robot.get_cMo();
276 planeToABC(pl, A, B, C);
281 m_moments->updateAll(obj);
284 m_featureMoments->updateAll(A, B, C);
287 m_robot.getInternalView(m_Iint);
294 v = m_task.computeControlLaw();
300 ViSP_plot.
plot(0, iter, v);
302 ViSP_plot.
plot(2, iter, m_task.getError());
304 m_error = (m_task.getError()).sumSquare();
314 m_robot.getInternalView(m_Iint);
343 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
345 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
346 std::cout <<
"Invalid position:" << std::endl;
347 std::cout << m_cMo << std::endl;
348 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
361 m_robot.setCurrentViewColor(
vpColor(150, 150, 150));
362 m_robot.setDesiredViewColor(
vpColor(200, 200, 200));
364 removeJointLimits(m_robot);
366 m_robot.setConstantSamplingTimeMode(
true);
369 m_robot.initialiseObjectRelativeToCamera(m_cMo);
372 m_robot.setDesiredCameraPosition(m_cdMo);
373 m_robot.getCameraParameters(m_cam, m_Iint);
378 double error() {
return m_error; }
380 void init_visp_plot(
vpPlot &ViSP_plot)
386 const unsigned int NbGraphs = 3;
387 const unsigned int NbCurves_in_graph[NbGraphs] = {6, 6, 6};
389 ViSP_plot.
init(NbGraphs, 800, 800, 100 + static_cast<int>(m_width), 50,
"Visual Servoing results...");
394 for (
unsigned int p = 0; p < NbGraphs; p++) {
395 ViSP_plot.
initGraph(p, NbCurves_in_graph[p]);
396 for (
unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
397 ViSP_plot.
setColor(p, c, Colors[c]);
400 ViSP_plot.
setTitle(0,
"Robot velocities");
408 ViSP_plot.
setTitle(1,
"Camera pose cMo");
416 ViSP_plot.
setTitle(2,
"Error in visual features: ");
427 unsigned int m_width;
428 unsigned int m_height;
454 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS 465 servo.init(cMo, cdMo);
469 std::cout <<
"Catch an exception: " << e << std::endl;
Class that defines generic functionnalities for display.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void init(unsigned int nbGraph, unsigned int height=700, unsigned int width=700, int x=-1, int y=-1, const std::string &title="")
Display for windows using GDI (available on any windows 32 platform).
Class to define RGB colors available for display functionnalities.
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
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.
error that can be emited by ViSP classes.
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
Class for generic objects.
static const vpColor green
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
static void flush(const vpImage< unsigned char > &I)
void setMaxRotationVelocity(double maxVr)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
static const vpColor orange
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
Initialize the velocity controller.
static const vpColor cyan
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.
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
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.
void setTitle(unsigned int graphNum, const std::string &title)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
vpServoIteractionMatrixType
void setABCD(double a, double b, double c, double d)
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
static double rad(double deg)
void initGraph(unsigned int graphNum, unsigned int curveNbr)
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
void setType(vpObjectType input_type)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
This class defines the container for a plane geometrical structure.
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
static const vpColor purple
void setMaxTranslationVelocity(double maxVt)
Class that consider the case of a translation vector.
static const vpColor blue