42#include <visp3/core/vpCameraParameters.h>
43#include <visp3/core/vpConfig.h>
44#include <visp3/core/vpDebug.h>
45#include <visp3/core/vpHomogeneousMatrix.h>
46#include <visp3/core/vpIoTools.h>
47#include <visp3/core/vpMath.h>
48#include <visp3/core/vpMomentCommon.h>
49#include <visp3/core/vpMomentDatabase.h>
50#include <visp3/core/vpMomentObject.h>
51#include <visp3/core/vpPlane.h>
52#include <visp3/gui/vpDisplayD3D.h>
53#include <visp3/gui/vpDisplayGDI.h>
54#include <visp3/gui/vpDisplayGTK.h>
55#include <visp3/gui/vpDisplayOpenCV.h>
56#include <visp3/gui/vpDisplayX.h>
57#include <visp3/gui/vpPlot.h>
58#include <visp3/robot/vpSimulatorAfma6.h>
59#include <visp3/visual_features/vpFeatureBuilder.h>
60#include <visp3/visual_features/vpFeatureMomentCommon.h>
61#include <visp3/visual_features/vpFeaturePoint.h>
62#include <visp3/vs/vpServo.h>
64#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
68 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is "
71 std::cout <<
"You should install pthread third-party library." << std::endl;
75#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
76 !defined(VISP_HAVE_GTK)
79 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
80 std::cout <<
"You should install one of the following third-party library: "
81 "X11, OpenCV, GDI, GTK."
87#ifndef DOXYGEN_SHOULD_SKIP_THIS
92 : m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(false), m_Iint(m_height, m_width, 255), m_task(), m_cam(),
93 m_error(0), m_imsim(), m_interaction_type(), m_src(6), m_dst(6), m_moments(NULL), m_momentsDes(NULL),
94 m_featureMoments(NULL), m_featureMomentsDes(NULL), m_displayInt(NULL)
98#ifdef VISP_HAVE_DISPLAY
105 delete m_featureMoments;
106 delete m_featureMomentsDes;
111 std::vector<vpPoint> src_pts;
112 std::vector<vpPoint> dst_pts;
114 double x[5] = { 0.2, 0.2, -0.2, -0.2, 0.2 };
115 double y[5] = { -0.1, 0.1, 0.1, -0.1, -0.1 };
118 for (
int i = 0; i < nbpoints; i++) {
121 src_pts.push_back(p);
125 m_src.fromVector(src_pts);
126 for (
int i = 0; i < nbpoints; i++) {
129 dst_pts.push_back(p);
132 m_dst.fromVector(dst_pts);
137 double x[5] = { 0.2, 0.2, -0.2, -0.2, 0.2 };
138 double y[5] = { -0.1, 0.1, 0.1, -0.1, -0.1 };
140 std::vector<vpPoint> cur_pts;
142 for (
int i = 0; i < nbpoints; i++) {
145 cur_pts.push_back(p);
157#ifdef VISP_HAVE_DISPLAY
159#if defined(VISP_HAVE_X11)
161#elif defined(HAVE_OPENCV_HIGHGUI)
163#elif defined(VISP_HAVE_GDI)
165#elif defined(VISP_HAVE_D3D9)
167#elif defined(VISP_HAVE_GTK)
170 m_displayInt->
init(m_Iint, 50, 50,
"Visual servoing with moments");
195 planeToABC(pl, A, B, C);
199 planeToABC(pl, Ad, Bd, Cd);
217 m_moments->updateAll(m_src);
218 m_momentsDes->updateAll(m_dst);
220 m_featureMoments->updateAll(A, B, C);
221 m_featureMomentsDes->updateAll(Ad, Bd, Cd);
224 m_task.setInteractionMatrixType(m_interaction_type);
227 m_task.addFeature(m_featureMoments->getFeatureGravityNormalized(),
228 m_featureMomentsDes->getFeatureGravityNormalized());
229 m_task.addFeature(m_featureMoments->getFeatureAn(), m_featureMomentsDes->getFeatureAn());
231 m_task.addFeature(m_featureMoments->getFeatureCInvariant(), m_featureMomentsDes->getFeatureCInvariant(),
232 (1 << 10) | (1 << 11));
233 m_task.addFeature(m_featureMoments->getFeatureAlpha(), m_featureMomentsDes->getFeatureAlpha());
235 m_task.setLambda(0.4);
238 void execute(
unsigned int nbIter)
241 init_visp_plot(ViSP_plot);
248 std::cout <<
"Display task information " << std::endl;
252 m_robot.getInternalView(m_Iint);
254 unsigned int iter = 0;
257 while (iter++ < nbIter) {
261 m_cMo = m_robot.get_cMo();
267 planeToABC(pl, A, B, C);
272 m_moments->updateAll(obj);
275 m_featureMoments->updateAll(A, B, C);
278 m_robot.getInternalView(m_Iint);
286 v = m_task.computeControlLaw();
290 ViSP_plot.
plot(0, iter, v);
292 ViSP_plot.
plot(2, iter, m_task.getError());
294 m_error = (m_task.getError()).sumSquare();
305 m_robot.getInternalView(m_Iint);
313 double error() {
return m_error; }
333 robot.setJointLimit(limMin, limMax);
336 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
338 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
339 std::cout <<
"Invalid position:" << std::endl;
340 std::cout << m_cMo << std::endl;
341 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
353 m_robot.setCurrentViewColor(
vpColor(150, 150, 150));
354 m_robot.setDesiredViewColor(
vpColor(200, 200, 200));
356 removeJointLimits(m_robot);
360 m_robot.initialiseObjectRelativeToCamera(m_cMo);
363 m_robot.setDesiredCameraPosition(m_cdMo);
364 m_robot.getCameraParameters(m_cam, m_Iint);
367 void init_visp_plot(
vpPlot &ViSP_plot)
373 const unsigned int NbGraphs = 3;
374 const unsigned int NbCurves_in_graph[NbGraphs] = { 6, 6, 6 };
376 ViSP_plot.
init(NbGraphs, 800, 800, 100 +
static_cast<int>(m_width), 50,
"Visual Servoing results...");
381 for (
unsigned int p = 0; p < NbGraphs; p++) {
382 ViSP_plot.
initGraph(p, NbCurves_in_graph[p]);
383 for (
unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
384 ViSP_plot.
setColor(p, c, Colors[c]);
387 ViSP_plot.
setTitle(0,
"Robot velocities");
395 ViSP_plot.
setTitle(1,
"Camera pose cMo");
403 ViSP_plot.
setTitle(2,
"Error in visual features: ");
414 unsigned int m_width;
415 unsigned int m_height;
452 servo.init(cMo, cdMo);
457 std::cout <<
"Catch an exception: " << e << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
static const vpColor cyan
static const vpColor orange
static const vpColor blue
static const vpColor purple
static const vpColor green
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Class that defines generic functionalities for display.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
@ divideByZeroError
Division by zero.
This class allows to access common vpFeatureMoments in a pre-filled database.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class which enables to project an image in the 3D space and get the view of a virtual camera.
Definition of the vpImage class member functions.
static double rad(double deg)
This class initializes and allows access to commonly used moments.
static std::vector< double > getMu3(vpMomentObject &object)
static double getAlpha(vpMomentObject &object)
static double getSurface(vpMomentObject &object)
Class for generic objects.
void setType(vpObjectType input_type)
void fromVector(std::vector< vpPoint > &points)
This class defines the container for a plane geometrical structure.
void changeFrame(const vpHomogeneousMatrix &cMo)
void setABCD(double a, double b, double c, double d)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void init(unsigned int nbGraph, unsigned int height=700, unsigned int width=700, int x=-1, int y=-1, const std::string &title="")
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
void setTitle(unsigned int graphNum, const std::string &title)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Implementation of a pose vector and operations on poses.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
vpServoIteractionMatrixType
Simulator of Irisa's gantry robot named Afma6.
Class that consider the case of a translation vector.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()