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