Visual Servoing Platform version 3.5.0
servoMomentPoints.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Example of visual servoing with moments using discrete points as object
33 * container
34 *
35 * Authors:
36 * Filip Novotny
37 *
38 *****************************************************************************/
39
45#include <iostream>
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>
67
68#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
69// Robot simulator used in this example is not available
70int main()
71{
72 std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
73 "not available."
74 << std::endl;
75 std::cout << "You should install pthread third-party library." << std::endl;
76 return EXIT_SUCCESS;
77}
78// No display available
79#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
80 !defined(VISP_HAVE_GTK)
81int main()
82{
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."
86 << std::endl;
87 return EXIT_SUCCESS;
88}
89#else
90
91#ifndef DOXYGEN_SHOULD_SKIP_THIS
92class servoMoment
93{
94public:
95 servoMoment()
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)
99 {
100 }
101 ~servoMoment()
102 {
103#ifdef VISP_HAVE_DISPLAY
104 if (m_displayInt) {
105 delete m_displayInt;
106 }
107#endif
108 delete m_moments;
109 delete m_momentsDes;
110 delete m_featureMoments;
111 delete m_featureMomentsDes;
112 }
113
114 // initialize scene in the interface
115 void initScene()
116 {
117 std::vector<vpPoint> src_pts;
118 std::vector<vpPoint> dst_pts;
119
120 double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
121 double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
122 int nbpoints = 8;
123
124 for (int i = 0; i < nbpoints; i++) {
125 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
126 p.track(m_cMo);
127 src_pts.push_back(p);
128 }
129
130 m_src.setType(vpMomentObject::DISCRETE);
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);
134 p.track(m_cdMo);
135 dst_pts.push_back(p);
136 }
137 m_dst.setType(vpMomentObject::DISCRETE);
138 m_dst.fromVector(dst_pts);
139 }
140
141 // initialize the moment features
142 void initFeatures()
143 {
144 // A,B,C parameters of source and destination plane
145 double A;
146 double B;
147 double C;
148 double Ad;
149 double Bd;
150 double Cd;
151 // init main object: using moments up to order 6
152
153 // Initializing values from regular plane (with ax+by+cz=d convention)
154 vpPlane pl;
155 pl.setABCD(0, 0, 1.0, 0);
156 pl.changeFrame(m_cMo);
157 planeToABC(pl, A, B, C);
158
159 pl.setABCD(0, 0, 1.0, 0);
160 pl.changeFrame(m_cdMo);
161 planeToABC(pl, Ad, Bd, Cd);
162
163 // extracting initial position (actually we only care about Zdst)
165 m_cdMo.extract(vec);
166
169 // don't need to be specific, vpMomentCommon automatically loads
170 // Xg,Yg,An,Ci,Cj,Alpha moments
172 vpMomentCommon::getAlpha(m_dst), vec[2]);
173 m_momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(m_dst), vpMomentCommon::getMu3(m_dst),
174 vpMomentCommon::getAlpha(m_dst), vec[2]);
175 // same thing with common features
176 m_featureMoments = new vpFeatureMomentCommon(*m_moments);
177 m_featureMomentsDes = new vpFeatureMomentCommon(*m_momentsDes);
178
179 m_moments->updateAll(m_src);
180 m_momentsDes->updateAll(m_dst);
181
182 m_featureMoments->updateAll(A, B, C);
183 m_featureMomentsDes->updateAll(Ad, Bd, Cd);
184
185 // setup the interaction type
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());
195
196 m_task.setLambda(1.);
197 }
198
199 // update moment objects and interface
200 void refreshScene(vpMomentObject &obj)
201 {
202 // double x[8] = { 0.05,0.15, 0.2,-0.05 ,-0.15,-0.1,-0.05,0.05};
203 // double y[8] = { 0,0.05, 0.2, 0.2, -0.1,-0.1, 0.05,0};
204 double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
205 double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
206 int nbpoints = 8;
207 std::vector<vpPoint> cur_pts;
208
209 for (int i = 0; i < nbpoints; i++) {
210 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
211 p.track(m_cMo);
212 cur_pts.push_back(p);
213 }
214 obj.fromVector(cur_pts);
215 }
216
217 void init(vpHomogeneousMatrix &cMo, vpHomogeneousMatrix &cdMo)
218 {
219 m_cMo = cMo; // init source matrix
220 m_cdMo = cdMo; // init destination matrix
221
222 m_interaction_type = vpServo::CURRENT; // use interaction matrix for current position
223
224#ifdef VISP_HAVE_DISPLAY
225 // init the right display
226#if defined VISP_HAVE_X11
227 m_displayInt = new vpDisplayX;
228#elif defined VISP_HAVE_OPENCV
229 m_displayInt = new vpDisplayOpenCV;
230#elif defined VISP_HAVE_GDI
231 m_displayInt = new vpDisplayGDI;
232#elif defined VISP_HAVE_D3D9
233 m_displayInt = new vpDisplayD3D;
234#elif defined VISP_HAVE_GTK
235 m_displayInt = new vpDisplayGTK;
236#endif
237 m_displayInt->init(m_Iint, 50, 50, "Visual servoing with moments");
238#endif
239
240 paramRobot(); // set up robot parameters
241
242 m_task.setServo(vpServo::EYEINHAND_CAMERA);
243 initScene(); // initialize graphical scene (for interface)
244 initFeatures(); // initialize moment features
245 }
246
247 // launch the simulation
248 void execute(unsigned int nbIter)
249 {
250 vpPlot ViSP_plot;
251 init_visp_plot(ViSP_plot); // Initialize plot object
252
253 // init main object: using moments up to order 6
254 vpMomentObject obj(6);
255 // setting object type (disrete, continuous[form polygon])
257
258 std::cout << "Display task information " << std::endl;
259 m_task.print();
260
261 vpDisplay::display(m_Iint);
262 m_robot.getInternalView(m_Iint);
263 vpDisplay::flush(m_Iint);
264 unsigned int iter = 0;
265
267 while (iter++ < nbIter) {
268 vpColVector v;
269 // get the cMo
270 m_cMo = m_robot.get_cMo();
271 // setup the plane in A,B,C style
272 vpPlane pl;
273 double A, B, C;
274 pl.setABCD(0, 0, 1.0, 0);
275 pl.changeFrame(m_cMo);
276 planeToABC(pl, A, B, C);
277
278 // track points, draw points and add refresh our object
279 refreshScene(obj);
280 // this is the most important thing to do: update our moments
281 m_moments->updateAll(obj);
282 // and update our features. Do it in that order. Features need to use the
283 // information computed by moments
284 m_featureMoments->updateAll(A, B, C);
285
286 vpDisplay::display(m_Iint);
287 m_robot.getInternalView(m_Iint);
288
289 if (iter == 1) {
290 vpDisplay::displayText(m_Iint, 20, 20, "Click to start servoing", vpColor::red);
291 vpDisplay::flush(m_Iint);
292 vpDisplay::getClick(m_Iint);
293 }
294 v = m_task.computeControlLaw();
295
296 // pilot robot using position control. The displacement is t*v with t=10ms
297 // step robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
298 m_robot.setVelocity(vpRobot::CAMERA_FRAME, v);
299
300 ViSP_plot.plot(0, iter, v);
301 ViSP_plot.plot(1, iter, vpPoseVector(m_cMo)); // Plot the velocities
302 ViSP_plot.plot(2, iter, m_task.getError()); // cMo as translations and theta_u
303
304 m_error = (m_task.getError()).sumSquare();
305
306 vpDisplay::displayText(m_Iint, 20, 20, "Click to stop visual servo...", vpColor::red);
307 if (vpDisplay::getClick(m_Iint, false)) {
308 break;
309 }
310 vpDisplay::flush(m_Iint);
311 }
312
313 vpDisplay::display(m_Iint);
314 m_robot.getInternalView(m_Iint);
315 vpDisplay::displayText(m_Iint, 20, 20, "Click to quit...", vpColor::red);
316 vpDisplay::flush(m_Iint);
317 vpDisplay::getClick(m_Iint);
318 }
319
320 void removeJointLimits(vpSimulatorAfma6 &robot)
321 {
322 vpColVector limMin(6);
323 vpColVector limMax(6);
324 limMin[0] = vpMath::rad(-3600);
325 limMin[1] = vpMath::rad(-3600);
326 limMin[2] = vpMath::rad(-3600);
327 limMin[3] = vpMath::rad(-3600);
328 limMin[4] = vpMath::rad(-3600);
329 limMin[5] = vpMath::rad(-3600);
330
331 limMax[0] = vpMath::rad(3600);
332 limMax[1] = vpMath::rad(3600);
333 limMax[2] = vpMath::rad(3600);
334 limMax[3] = vpMath::rad(3600);
335 limMax[4] = vpMath::rad(3600);
336 limMax[5] = vpMath::rad(3600);
337
338 robot.setJointLimit(limMin, limMax);
339 robot.setMaxRotationVelocity(99999);
340 robot.setMaxTranslationVelocity(999999);
341 }
342
343 void planeToABC(vpPlane &pl, double &A, double &B, double &C)
344 {
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;
349 throw vpException(vpException::divideByZeroError, "invalid position!");
350 }
351 A = -pl.getA() / pl.getD();
352 B = -pl.getB() / pl.getD();
353 C = -pl.getC() / pl.getD();
354 }
355
356 // setup robot parameters
357 void paramRobot()
358 {
359 /*Initialise the robot and especially the camera*/
361 m_robot.setCurrentViewColor(vpColor(150, 150, 150));
362 m_robot.setDesiredViewColor(vpColor(200, 200, 200));
363 m_robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
364 removeJointLimits(m_robot);
366 m_robot.setConstantSamplingTimeMode(true);
367 /*Initialise the position of the object relative to the pose of the robot's
368 * camera*/
369 m_robot.initialiseObjectRelativeToCamera(m_cMo);
370
371 /*Set the desired position (for the displaypart)*/
372 m_robot.setDesiredCameraPosition(m_cdMo);
373 m_robot.getCameraParameters(m_cam, m_Iint);
374 }
375
376 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { m_interaction_type = type; }
377
378 double error() { return m_error; }
379
380 void init_visp_plot(vpPlot &ViSP_plot)
381 {
382 /* -------------------------------------
383 * Initialize ViSP Plotting
384 * -------------------------------------
385 */
386 const unsigned int NbGraphs = 3; // No. of graphs
387 const unsigned int NbCurves_in_graph[NbGraphs] = {6, 6, 6}; // Curves in each graph
388
389 ViSP_plot.init(NbGraphs, 800, 800, 100 + static_cast<int>(m_width), 50, "Visual Servoing results...");
390
391 vpColor Colors[6] = {// Colour for s1, s2, s3, in 1st plot
393
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]);
398 }
399
400 ViSP_plot.setTitle(0, "Robot velocities");
401 ViSP_plot.setLegend(0, 0, "v_x");
402 ViSP_plot.setLegend(0, 1, "v_y");
403 ViSP_plot.setLegend(0, 2, "v_z");
404 ViSP_plot.setLegend(0, 3, "w_x");
405 ViSP_plot.setLegend(0, 4, "w_y");
406 ViSP_plot.setLegend(0, 5, "w_z");
407
408 ViSP_plot.setTitle(1, "Camera pose cMo");
409 ViSP_plot.setLegend(1, 0, "tx");
410 ViSP_plot.setLegend(1, 1, "ty");
411 ViSP_plot.setLegend(1, 2, "tz");
412 ViSP_plot.setLegend(1, 3, "tu_x");
413 ViSP_plot.setLegend(1, 4, "tu_y");
414 ViSP_plot.setLegend(1, 5, "tu_z");
415
416 ViSP_plot.setTitle(2, "Error in visual features: ");
417 ViSP_plot.setLegend(2, 0, "x_n");
418 ViSP_plot.setLegend(2, 1, "y_n");
419 ViSP_plot.setLegend(2, 2, "a_n");
420 ViSP_plot.setLegend(2, 3, "sx");
421 ViSP_plot.setLegend(2, 4, "sy");
422 ViSP_plot.setLegend(2, 5, "alpha");
423 }
424
425protected:
426 // start and destination positioning matrices
427 unsigned int m_width;
428 unsigned int m_height;
429
430 // start and destination positioning matrices
432 vpHomogeneousMatrix m_cdMo;
433
434 vpSimulatorAfma6 m_robot; // robot used in this simulation
435 vpImage<vpRGBa> m_Iint; // internal image used for interface display
436 vpServo m_task; // servoing task
437 vpCameraParameters m_cam; // robot camera parameters
438 double m_error; // current error
439 vpImageSimulator m_imsim; // image simulator used to simulate the perspective-projection camera
440
441 vpServo::vpServoIteractionMatrixType m_interaction_type; // current or desired
442 // source and destination objects for moment manipulation
443 vpMomentObject m_src;
444 vpMomentObject m_dst;
445
446 // moment sets and their corresponding features
447 vpMomentCommon *m_moments;
448 vpMomentCommon *m_momentsDes;
449 vpFeatureMomentCommon *m_featureMoments;
450 vpFeatureMomentCommon *m_featureMomentsDes;
451
452 vpDisplay *m_displayInt;
453};
454#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
455
456int main()
457{
458 try { // intial pose
459 vpHomogeneousMatrix cMo(0.05, 0.1, 1.5, vpMath::rad(30), vpMath::rad(20), -vpMath::rad(15));
460 // Desired pose
462
463 servoMoment servo;
464 // init and run the simulation
465 servo.init(cMo, cdMo);
466 servo.execute(1500);
467 return EXIT_SUCCESS;
468 } catch (const vpException &e) {
469 std::cout << "Catch an exception: " << e << std::endl;
470 return EXIT_FAILURE;
471 }
472}
473
474#endif
@ TOOL_CCMOP
Definition: vpAfma6.h:127
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor orange
Definition: vpColor.h:227
static const vpColor blue
Definition: vpColor.h:223
static const vpColor purple
Definition: vpColor.h:228
static const vpColor green
Definition: vpColor.h:220
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:107
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
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...
Definition: vpDisplayX.h:135
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:178
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 emited by ViSP classes.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
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.
static double rad(double deg)
Definition: vpMath.h:110
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.
Definition: vpPlane.h:59
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
double getD() const
Definition: vpPlane.h:108
double getA() const
Definition: vpPlane.h:102
double getC() const
Definition: vpPlane.h:106
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:90
double getB() const
Definition: vpPlane.h:104
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:116
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:206
void init(unsigned int nbGraph, unsigned int height=700, unsigned int width=700, int x=-1, int y=-1, const std::string &title="")
Definition: vpPlot.cpp:100
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:286
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
Definition: vpPlot.cpp:261
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:239
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
vpServoIteractionMatrixType
Definition: vpServo.h:181
@ CURRENT
Definition: vpServo.h:182
Simulator of Irisa's gantry robot named Afma6.
Class that consider the case of a translation vector.