Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoMomentPolygon.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 a polygon as object container
33 *
34*****************************************************************************/
35
41#include <iostream>
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>
63
64#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
65// Robot simulator used in this example is not available
66int main()
67{
68 std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
69 "not available."
70 << std::endl;
71 std::cout << "You should install pthread third-party library." << std::endl;
72 return EXIT_SUCCESS;
73}
74// No display available
75#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
76 !defined(VISP_HAVE_GTK)
77int main()
78{
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."
82 << std::endl;
83 return EXIT_SUCCESS;
84}
85#else
86
87#ifndef DOXYGEN_SHOULD_SKIP_THIS
88class servoMoment
89{
90public:
91 servoMoment()
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)
95 { }
96 ~servoMoment()
97 {
98#ifdef VISP_HAVE_DISPLAY
99 if (m_displayInt) {
100 delete m_displayInt;
101 }
102#endif
103 delete m_moments;
104 delete m_momentsDes;
105 delete m_featureMoments;
106 delete m_featureMomentsDes;
107 }
108
109 void initScene()
110 {
111 std::vector<vpPoint> src_pts;
112 std::vector<vpPoint> dst_pts;
113
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 };
116 int nbpoints = 4;
117
118 for (int i = 0; i < nbpoints; i++) {
119 vpPoint p(x[i], y[i], 0.0);
120 p.track(m_cMo);
121 src_pts.push_back(p);
122 }
123
124 m_src.setType(vpMomentObject::DENSE_POLYGON);
125 m_src.fromVector(src_pts);
126 for (int i = 0; i < nbpoints; i++) {
127 vpPoint p(x[i], y[i], 0.0);
128 p.track(m_cdMo);
129 dst_pts.push_back(p);
130 }
131 m_dst.setType(vpMomentObject::DENSE_POLYGON);
132 m_dst.fromVector(dst_pts);
133 }
134
135 void refreshScene(vpMomentObject &obj)
136 {
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 };
139 int nbpoints = 5;
140 std::vector<vpPoint> cur_pts;
141
142 for (int i = 0; i < nbpoints; i++) {
143 vpPoint p(x[i], y[i], 0.0);
144 p.track(m_cMo);
145 cur_pts.push_back(p);
146 }
147 obj.fromVector(cur_pts);
148 }
149
150 void init(vpHomogeneousMatrix &cMo, vpHomogeneousMatrix &cdMo)
151 {
152 m_cMo = cMo; // init source matrix
153 m_cdMo = cdMo; // init destination matrix
154
155 m_interaction_type = vpServo::CURRENT; // use interaction matrix for current position
156
157#ifdef VISP_HAVE_DISPLAY
158 // init the right display
159#if defined(VISP_HAVE_X11)
160 m_displayInt = new vpDisplayX;
161#elif defined(HAVE_OPENCV_HIGHGUI)
162 m_displayInt = new vpDisplayOpenCV;
163#elif defined(VISP_HAVE_GDI)
164 m_displayInt = new vpDisplayGDI;
165#elif defined(VISP_HAVE_D3D9)
166 m_displayInt = new vpDisplayD3D;
167#elif defined(VISP_HAVE_GTK)
168 m_displayInt = new vpDisplayGTK;
169#endif
170 m_displayInt->init(m_Iint, 50, 50, "Visual servoing with moments");
171#endif
172
173 paramRobot(); // set up robot parameters
174
175 m_task.setServo(vpServo::EYEINHAND_CAMERA);
176 initScene(); // initialize graphical scene (for interface)
177 initFeatures(); // initialize moment features
178 }
179
180 void initFeatures()
181 {
182 // A,B,C parameters of source and destination plane
183 double A;
184 double B;
185 double C;
186 double Ad;
187 double Bd;
188 double Cd;
189 // init main object: using moments up to order 6
190
191 // Initializing values from regular plane (with ax+by+cz=d convention)
192 vpPlane pl;
193 pl.setABCD(0, 0, 1.0, 0);
194 pl.changeFrame(m_cMo);
195 planeToABC(pl, A, B, C);
196
197 pl.setABCD(0, 0, 1.0, 0);
198 pl.changeFrame(m_cdMo);
199 planeToABC(pl, Ad, Bd, Cd);
200
201 // extracting initial position (actually we only care about Zdst)
203 m_cdMo.extract(vec);
204
207 // don't need to be specific, vpMomentCommon automatically loads
208 // Xg,Yg,An,Ci,Cj,Alpha moments
210 vpMomentCommon::getAlpha(m_dst), vec[2]);
211 m_momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(m_dst), vpMomentCommon::getMu3(m_dst),
212 vpMomentCommon::getAlpha(m_dst), vec[2]);
213 // same thing with common features
214 m_featureMoments = new vpFeatureMomentCommon(*m_moments);
215 m_featureMomentsDes = new vpFeatureMomentCommon(*m_momentsDes);
216
217 m_moments->updateAll(m_src);
218 m_momentsDes->updateAll(m_dst);
219
220 m_featureMoments->updateAll(A, B, C);
221 m_featureMomentsDes->updateAll(Ad, Bd, Cd);
222
223 // setup the interaction type
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());
230 // the moments are different in case of a symmetric object
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());
234
235 m_task.setLambda(0.4);
236 }
237
238 void execute(unsigned int nbIter)
239 {
240 vpPlot ViSP_plot;
241 init_visp_plot(ViSP_plot); // Initialize plot object
242
243 // init main object: using moments up to order 5
244 vpMomentObject obj(6);
245 // setting object type (disrete, continuous[form polygon])
247
248 std::cout << "Display task information " << std::endl;
249 m_task.print();
250
251 vpDisplay::display(m_Iint);
252 m_robot.getInternalView(m_Iint);
253 vpDisplay::flush(m_Iint);
254 unsigned int iter = 0;
255
257 while (iter++ < nbIter) {
258 vpColVector v;
259 double t = vpTime::measureTimeMs();
260 // get the cMo
261 m_cMo = m_robot.get_cMo();
262 // setup the plane in A,B,C style
263 vpPlane pl;
264 double A, B, C;
265 pl.setABCD(0, 0, 1.0, 0);
266 pl.changeFrame(m_cMo);
267 planeToABC(pl, A, B, C);
268
269 // track points, draw points and add refresh our object
270 refreshScene(obj);
271 // this is the most important thing to do: update our moments
272 m_moments->updateAll(obj);
273 // and update our features. Do it in that order. Features need to use the
274 // information computed by moments
275 m_featureMoments->updateAll(A, B, C);
276
277 vpDisplay::display(m_Iint);
278 m_robot.getInternalView(m_Iint);
279 vpDisplay::flush(m_Iint);
280
281 if (iter == 1) {
282 vpDisplay::displayText(m_Iint, 20, 20, "Click to start servoing", vpColor::red);
283 vpDisplay::flush(m_Iint);
284 vpDisplay::getClick(m_Iint);
285 }
286 v = m_task.computeControlLaw();
287
288 m_robot.setVelocity(vpRobot::CAMERA_FRAME, v);
289
290 ViSP_plot.plot(0, iter, v);
291 ViSP_plot.plot(1, iter, vpPoseVector(m_cMo)); // Plot the velocities
292 ViSP_plot.plot(2, iter, m_task.getError()); // cMo as translations and theta_u
293
294 m_error = (m_task.getError()).sumSquare();
295
296 vpDisplay::displayText(m_Iint, 20, 20, "Click to stop visual servo...", vpColor::red);
297 if (vpDisplay::getClick(m_Iint, false)) {
298 break;
299 }
300 vpDisplay::flush(m_Iint);
301 vpTime::wait(t, 10);
302 }
303
304 vpDisplay::display(m_Iint);
305 m_robot.getInternalView(m_Iint);
306 vpDisplay::displayText(m_Iint, 20, 20, "Click to quit...", vpColor::red);
307 vpDisplay::flush(m_Iint);
308 vpDisplay::getClick(m_Iint);
309 }
310
311 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { m_interaction_type = type; }
312
313 double error() { return m_error; }
314
315 void removeJointLimits(vpSimulatorAfma6 &robot)
316 {
317 vpColVector limMin(6);
318 vpColVector limMax(6);
319 limMin[0] = vpMath::rad(-3600);
320 limMin[1] = vpMath::rad(-3600);
321 limMin[2] = vpMath::rad(-3600);
322 limMin[3] = vpMath::rad(-3600);
323 limMin[4] = vpMath::rad(-3600);
324 limMin[5] = vpMath::rad(-3600);
325
326 limMax[0] = vpMath::rad(3600);
327 limMax[1] = vpMath::rad(3600);
328 limMax[2] = vpMath::rad(3600);
329 limMax[3] = vpMath::rad(3600);
330 limMax[4] = vpMath::rad(3600);
331 limMax[5] = vpMath::rad(3600);
332
333 robot.setJointLimit(limMin, limMax);
334 }
335
336 void planeToABC(vpPlane &pl, double &A, double &B, double &C)
337 {
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;
342 throw vpException(vpException::divideByZeroError, "invalid position!");
343 }
344 A = -pl.getA() / pl.getD();
345 B = -pl.getB() / pl.getD();
346 C = -pl.getC() / pl.getD();
347 }
348
349 void paramRobot()
350 {
351 /*Initialise the robot and especially the camera*/
353 m_robot.setCurrentViewColor(vpColor(150, 150, 150));
354 m_robot.setDesiredViewColor(vpColor(200, 200, 200));
355 m_robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
356 removeJointLimits(m_robot);
358 /*Initialise the position of the object relative to the pose of the robot's
359 * camera*/
360 m_robot.initialiseObjectRelativeToCamera(m_cMo);
361
362 /*Set the desired position (for the displaypart)*/
363 m_robot.setDesiredCameraPosition(m_cdMo);
364 m_robot.getCameraParameters(m_cam, m_Iint);
365 }
366
367 void init_visp_plot(vpPlot &ViSP_plot)
368 {
369 /* -------------------------------------
370 * Initialize ViSP Plotting
371 * -------------------------------------
372 */
373 const unsigned int NbGraphs = 3; // No. of graphs
374 const unsigned int NbCurves_in_graph[NbGraphs] = { 6, 6, 6 }; // Curves in each graph
375
376 ViSP_plot.init(NbGraphs, 800, 800, 100 + static_cast<int>(m_width), 50, "Visual Servoing results...");
377
378 vpColor Colors[6] = {// Colour for s1, s2, s3, in 1st plot
380
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]);
385 }
386
387 ViSP_plot.setTitle(0, "Robot velocities");
388 ViSP_plot.setLegend(0, 0, "v_x");
389 ViSP_plot.setLegend(0, 1, "v_y");
390 ViSP_plot.setLegend(0, 2, "v_z");
391 ViSP_plot.setLegend(0, 3, "w_x");
392 ViSP_plot.setLegend(0, 4, "w_y");
393 ViSP_plot.setLegend(0, 5, "w_z");
394
395 ViSP_plot.setTitle(1, "Camera pose cMo");
396 ViSP_plot.setLegend(1, 0, "tx");
397 ViSP_plot.setLegend(1, 1, "ty");
398 ViSP_plot.setLegend(1, 2, "tz");
399 ViSP_plot.setLegend(1, 3, "tu_x");
400 ViSP_plot.setLegend(1, 4, "tu_y");
401 ViSP_plot.setLegend(1, 5, "tu_z");
402
403 ViSP_plot.setTitle(2, "Error in visual features: ");
404 ViSP_plot.setLegend(2, 0, "x_n");
405 ViSP_plot.setLegend(2, 1, "y_n");
406 ViSP_plot.setLegend(2, 2, "a_n");
407 ViSP_plot.setLegend(2, 3, "sx");
408 ViSP_plot.setLegend(2, 4, "sy");
409 ViSP_plot.setLegend(2, 5, "alpha");
410 }
411
412protected:
413 // start and destination positioning matrices
414 unsigned int m_width;
415 unsigned int m_height;
416
417 // start and destination positioning matrices
419 vpHomogeneousMatrix m_cdMo;
420
421 vpSimulatorAfma6 m_robot; // robot used in this simulation
422 vpImage<vpRGBa> m_Iint; // internal image used for interface display
423 vpServo m_task; // servoing task
424 vpCameraParameters m_cam; // robot camera parameters
425 double m_error; // current error
426 vpImageSimulator m_imsim; // image simulator used to simulate the perspective-projection camera
427
428 vpServo::vpServoIteractionMatrixType m_interaction_type; // current or desired
429 // source and destination objects for moment manipulation
430 vpMomentObject m_src;
431 vpMomentObject m_dst;
432
433 // moment sets and their corresponding features
434 vpMomentCommon *m_moments;
435 vpMomentCommon *m_momentsDes;
436 vpFeatureMomentCommon *m_featureMoments;
437 vpFeatureMomentCommon *m_featureMomentsDes;
438
439 vpDisplay *m_displayInt;
440};
441#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
442
443int main()
444{
445 try { // intial pose
446 vpHomogeneousMatrix cMo(-0.1, -0.1, 1.5, -vpMath::rad(20), -vpMath::rad(20), -vpMath::rad(30));
447 // Desired pose
449
450 servoMoment servo;
451 // init and run the simulation
452 servo.init(cMo, cdMo);
453 servo.execute(1500);
454 return EXIT_SUCCESS;
455 }
456 catch (const vpException &e) {
457 std::cout << "Catch an exception: " << e << std::endl;
458 return EXIT_FAILURE;
459 }
460}
461
462#endif
@ TOOL_CCMOP
Definition vpAfma6.h:124
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.
Definition vpColor.h:152
static const vpColor red
Definition vpColor.h:211
static const vpColor cyan
Definition vpColor.h:220
static const vpColor orange
Definition vpColor.h:221
static const vpColor blue
Definition vpColor.h:217
static const vpColor purple
Definition vpColor.h:222
static const vpColor green
Definition vpColor.h:214
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...
Definition vpDisplayX.h:132
Class that defines generic functionalities for display.
Definition vpDisplay.h:173
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.
Definition vpException.h:59
@ divideByZeroError
Division by zero.
Definition vpException.h:82
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.
Definition vpImage.h:135
static double rad(double deg)
Definition vpMath.h:116
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:54
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition vpPlane.cpp:361
double getD() const
Definition vpPlane.h:106
double getA() const
Definition vpPlane.h:100
double getC() const
Definition vpPlane.h:104
void setABCD(double a, double b, double c, double d)
Definition vpPlane.h:88
double getB() const
Definition vpPlane.h:102
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition vpPlot.h:113
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition vpPlot.cpp:202
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:95
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition vpPlot.cpp:545
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition vpPlot.cpp:269
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
Definition vpPlot.cpp:245
void setTitle(unsigned int graphNum, const std::string &title)
Definition vpPlot.cpp:503
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
Implementation of a pose vector and operations on poses.
@ CAMERA_FRAME
Definition vpRobot.h:80
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ EYEINHAND_CAMERA
Definition vpServo.h:151
vpServoIteractionMatrixType
Definition vpServo.h:178
@ CURRENT
Definition vpServo.h:179
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()