Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoViper850Point2DArtVelocity-jointAvoidance-gpa.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 * tests the control law
33 * eye-in-hand control
34 * velocity computed in articular
35 *
36*****************************************************************************/
37
46#include <visp3/core/vpConfig.h>
47#include <visp3/core/vpDebug.h> // Debug trace
48
49#include <fstream>
50#include <iostream>
51#include <sstream>
52#include <stdio.h>
53#include <stdlib.h>
54
55#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_DISPLAY))
56
57#include <visp3/blob/vpDot2.h>
58#include <visp3/core/vpDisplay.h>
59#include <visp3/core/vpException.h>
60#include <visp3/core/vpHomogeneousMatrix.h>
61#include <visp3/core/vpImage.h>
62#include <visp3/core/vpIoTools.h>
63#include <visp3/core/vpMath.h>
64#include <visp3/core/vpPoint.h>
65#include <visp3/gui/vpDisplayGTK.h>
66#include <visp3/gui/vpDisplayOpenCV.h>
67#include <visp3/gui/vpDisplayX.h>
68#include <visp3/gui/vpPlot.h>
69#include <visp3/robot/vpRobotViper850.h>
70#include <visp3/sensor/vp1394TwoGrabber.h>
71#include <visp3/visual_features/vpFeatureBuilder.h>
72#include <visp3/visual_features/vpFeaturePoint.h>
73#include <visp3/vs/vpServo.h>
74#include <visp3/vs/vpServoDisplay.h>
75
76int main()
77{
78 try {
79 vpRobotViper850 robot;
80
81 vpServo task;
82
84
85 bool reset = false;
86 vp1394TwoGrabber g(reset);
89 g.open(I);
90
91 g.acquire(I);
92
93#ifdef VISP_HAVE_X11
94 vpDisplayX display(I, 800, 100, "Current image");
95#elif defined(HAVE_OPENCV_HIGHGUI)
96 vpDisplayOpenCV display(I, 800, 100, "Current image");
97#elif defined(VISP_HAVE_GTK)
98 vpDisplayGTK display(I, 800, 100, "Current image");
99#endif
100
103
104 vpColVector jointMin(6), jointMax(6);
105 jointMin = robot.getJointMin();
106 jointMax = robot.getJointMax();
107
108 vpColVector Qmin(6), tQmin(6);
109 vpColVector Qmax(6), tQmax(6);
110 vpColVector Qmiddle(6);
111 vpColVector data(10);
112
113 double rho = 0.15;
114 for (unsigned int i = 0; i < 6; i++) {
115 Qmin[i] = jointMin[i] + 0.5 * rho * (jointMax[i] - jointMin[i]);
116 Qmax[i] = jointMax[i] - 0.5 * rho * (jointMax[i] - jointMin[i]);
117 }
118 Qmiddle = (Qmin + Qmax) / 2.;
119 double rho1 = 0.1;
120
121 for (unsigned int i = 0; i < 6; i++) {
122 tQmin[i] = Qmin[i] + 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
123 tQmax[i] = Qmax[i] - 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
124 }
125
126 vpColVector q(6);
127
128 // Create a window with two graphics
129 // - first graphic to plot q(t), Qmin, Qmax, tQmin and tQmax
130 // - second graphic to plot the cost function h_s
131 vpPlot plot(2);
132
133 // The first graphic contains 10 data to plot: q(t), Qmin, Qmax, tQmin and
134 // tQmax
135 plot.initGraph(0, 10);
136 // The second graphic contains 1 curve, the cost function h_s
137 plot.initGraph(1, 1);
138
139 // For the first graphic :
140 // - along the x axis the expected values are between 0 and 200
141 // - along the y axis the expected values are between -1.2 and 1.2
142 plot.initRange(0, 0., 200., -1.2, 1.2);
143 plot.setTitle(0, "Joint behavior");
144
145 // For the second graphic :
146 // - along the x axis the expected values are between 0 and 200 and
147 // the step is 1
148 // - along the y axis the expected values are between 0 and 0.0001 and the
149 // step is 0.00001
150 plot.initRange(1, 0., 200., 0., 1e-4);
151 plot.setTitle(1, "Cost function");
152
153 // For the first graphic, set the curves legend
154 std::string legend;
155 for (unsigned int i = 0; i < 6; i++) {
156 legend = "q" + i + 1;
157 plot.setLegend(0, i, legend);
158 }
159 plot.setLegend(0, 6, "tQmin");
160 plot.setLegend(0, 7, "tQmax");
161 plot.setLegend(0, 8, "Qmin");
162 plot.setLegend(0, 9, "Qmax");
163
164 // Set the curves color
165 plot.setColor(0, 0, vpColor::red);
166 plot.setColor(0, 1, vpColor::green);
167 plot.setColor(0, 2, vpColor::blue);
168 plot.setColor(0, 3, vpColor::orange);
169 plot.setColor(0, 4, vpColor(0, 128, 0));
170 plot.setColor(0, 5, vpColor::cyan);
171 for (unsigned int i = 6; i < 10; i++)
172 plot.setColor(0, i, vpColor::black); // for Q and tQ [min,max]
173
174 // For the second graphic, set the curves legend
175 plot.setLegend(1, 0, "h_s");
176
177 double beta = 1;
178
179 // Set the amplitude of the control law due to the secondary task
180 std::cout << " Give the parameters beta (1) : ";
181 std::cin >> beta;
182
183 vpDot2 dot;
184
185 std::cout << "Click on a dot..." << std::endl;
186 dot.initTracking(I);
187 vpImagePoint cog = dot.getCog();
190
192 // Update camera parameters
193 robot.getCameraParameters(cam, I);
194
195 // sets the current position of the visual feature
197 vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
198
199 p.set_Z(1);
200 // sets the desired position of the visual feature
202 pd.buildFrom(0, 0, 1);
203
204 // Define the task
205 // - we want an eye-in-hand control law
206 // - articular velocity are computed
209
211 robot.get_cVe(cVe);
212 std::cout << cVe << std::endl;
213 task.set_cVe(cVe);
214
215 // - Set the Jacobian (expressed in the end-effector frame)") ;
216 vpMatrix eJe;
217 robot.get_eJe(eJe);
218 task.set_eJe(eJe);
219
220 // - we want to see a point on a point..") ;
221 std::cout << std::endl;
222 task.addFeature(p, pd);
223
224 // - set the gain
225 task.setLambda(0.8);
226
227 // Display task information " ) ;
228 task.print();
229
231
232 int iter = 0;
233 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
234 for (;;) {
235 iter++;
236 // Acquire a new image from the camera
237 g.acquire(I);
238
239 // Display this image
241
242 // Achieve the tracking of the dot in the image
243 dot.track(I);
244 cog = dot.getCog();
245
246 // Display a green cross at the center of gravity position in the image
248
249 // Get the measured joint positions of the robot
250 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
251
252 // Update the point feature from the dot location
253 vpFeatureBuilder::create(p, cam, dot);
254
255 // Get the jacobian of the robot
256 robot.get_eJe(eJe);
257 // Update this jacobian in the task structure. It will be used to
258 // compute the velocity skew (as an articular velocity) qdot = -lambda *
259 // L^+ * cVe * eJe * (s-s*)
260 task.set_eJe(eJe);
261
262 vpColVector prim_task;
263 vpColVector e2(6);
264 // Compute the visual servoing skew vector
265 prim_task = task.computeControlLaw();
266
267 vpColVector sec_task(6);
268 double h_s = 0;
269 {
270 // joint limit avoidance with secondary task
271
272 vpColVector de2dt(6);
273 de2dt = 0;
274 e2 = 0;
275 for (unsigned int i = 0; i < 6; i++) {
276 double S = 0;
277 if (q[i] > tQmax[i])
278 S = q[i] - tQmax[i];
279 if (q[i] < tQmin[i])
280 S = q[i] - tQmin[i];
281 double D = (Qmax[i] - Qmin[i]);
282 h_s += vpMath::sqr(S) / D;
283 e2[i] = S / D;
284 }
285 h_s = beta * h_s / 2.0; // cost function
286 e2 *= beta;
287 std::cout << "Cost function h_s: " << h_s << std::endl;
288
289 sec_task = task.secondaryTask(e2, de2dt);
290 }
291
292 vpColVector v;
293 v = prim_task + sec_task;
294
295 // Display the current and desired feature points in the image display
296 vpServoDisplay::display(task, cam, I);
297
298 // Apply the computed joint velocities to the robot
300
301 {
302 // Add the material to plot curves
303
304 // q normalized between (entre -1 et 1)
305 for (unsigned int i = 0; i < 6; i++) {
306 data[i] = (q[i] - Qmiddle[i]);
307 data[i] /= (Qmax[i] - Qmin[i]);
308 data[i] *= 2;
309 }
310 unsigned int joint = 2;
311 data[6] = 2 * (tQmin[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
312 data[7] = 2 * (tQmax[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
313 data[8] = -1;
314 data[9] = 1;
315 plot.plot(0, iter, data); // plot q, Qmin, Qmax, tQmin, tQmax
316 plot.plot(1, 0, iter, h_s); // plot the cost function
317 }
318
320 }
321
322 // Display task information
323 task.print();
324 return EXIT_SUCCESS;
325 } catch (const vpException &e) {
326 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
327 return EXIT_FAILURE;
328 }
329}
330
331#else
332int main()
333{
334 std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
335 return EXIT_SUCCESS;
336}
337#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Generic class defining intrinsic camera parameters.
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 black
Definition vpColor.h:205
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 green
Definition vpColor.h:214
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
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
static void display(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition vpDot2.h:124
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition vpDot2.cpp:441
vpImagePoint getCog() const
Definition vpDot2.h:177
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition vpDot2.cpp:252
error that can be emitted by ViSP classes.
Definition vpException.h:59
const char * getMessage() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
void set_Z(double Z)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_L_cVe_eJe
Definition vpServo.h:155
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition vpServo.h:448
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition vpServo.cpp:299
void setLambda(double c)
Definition vpServo.h:403
void set_eJe(const vpMatrix &eJe_)
Definition vpServo.h:506
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition vpServo.cpp:1454
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
@ PSEUDO_INVERSE
Definition vpServo.h:199
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ DESIRED
Definition vpServo.h:183
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
vpVelocityTwistMatrix get_cVe() const
Definition vpUnicycle.h:79