Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoViper850Point2DArtVelocity-jointAvoidance-basic.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
47#include <visp3/core/vpConfig.h>
48#include <visp3/core/vpDebug.h> // Debug trace
49
50#include <cmath> // std::fabs
51#include <fstream>
52#include <iostream>
53#include <limits> // numeric_limits
54#include <sstream>
55#include <stdio.h>
56#include <stdlib.h>
57
58#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_DISPLAY))
59
60#include <visp3/blob/vpDot2.h>
61#include <visp3/core/vpDisplay.h>
62#include <visp3/core/vpException.h>
63#include <visp3/core/vpHomogeneousMatrix.h>
64#include <visp3/core/vpImage.h>
65#include <visp3/core/vpIoTools.h>
66#include <visp3/core/vpMath.h>
67#include <visp3/core/vpPoint.h>
68#include <visp3/gui/vpDisplayGTK.h>
69#include <visp3/gui/vpDisplayOpenCV.h>
70#include <visp3/gui/vpDisplayX.h>
71#include <visp3/gui/vpPlot.h>
72#include <visp3/robot/vpRobotViper850.h>
73#include <visp3/sensor/vp1394TwoGrabber.h>
74#include <visp3/visual_features/vpFeatureBuilder.h>
75#include <visp3/visual_features/vpFeaturePoint.h>
76#include <visp3/vs/vpServo.h>
77#include <visp3/vs/vpServoDisplay.h>
78
79int main()
80{
81 try {
82 vpRobotViper850 robot;
83
84 vpServo task;
85
87
88 bool reset = false;
89 vp1394TwoGrabber g(reset);
92 g.open(I);
93
94 g.acquire(I);
95
96 double Tloop = 1. / 60.f;
97
99 g.getFramerate(fps);
100 switch (fps) {
102 Tloop = 1.f / 15.f;
103 break;
105 Tloop = 1.f / 30.f;
106 break;
108 Tloop = 1.f / 60.f;
109 break;
111 Tloop = 1.f / 120.f;
112 break;
113 default:
114 break;
115 }
116 std::cout << "Tloop: " << Tloop << std::endl;
117
118#ifdef VISP_HAVE_X11
119 vpDisplayX display(I, 800, 100, "Current image");
120#elif defined(HAVE_OPENCV_HIGHGUI)
121 vpDisplayOpenCV display(I, 800, 100, "Current image");
122#elif defined(VISP_HAVE_GTK)
123 vpDisplayGTK display(I, 800, 100, "Current image");
124#endif
125
128
129 vpColVector jointMin(6), jointMax(6);
130 jointMin = robot.getJointMin();
131 jointMax = robot.getJointMax();
132
133 vpColVector Qmin(6), tQmin(6);
134 vpColVector Qmax(6), tQmax(6);
135 vpColVector Qmiddle(6);
136 vpColVector data(10);
137
138 double rho = 0.25;
139 for (unsigned int i = 0; i < 6; i++) {
140 Qmin[i] = jointMin[i] + 0.5 * rho * (jointMax[i] - jointMin[i]);
141 Qmax[i] = jointMax[i] - 0.5 * rho * (jointMax[i] - jointMin[i]);
142 }
143 Qmiddle = (Qmin + Qmax) / 2.;
144 double rho1 = 0.1;
145
146 for (unsigned int i = 0; i < 6; i++) {
147 tQmin[i] = Qmin[i] + 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
148 tQmax[i] = Qmax[i] - 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
149 }
150
151 vpColVector q(6);
152
153 // Create a window with two graphics
154 // - first graphic to plot q(t), Qmin, Qmax, tQmin and tQmax
155 // - second graphic to plot the cost function h_s
156 vpPlot plot(2);
157
158 // The first graphic contains 10 data to plot: q(t), Qmin, Qmax, tQmin and
159 // tQmax
160 plot.initGraph(0, 10);
161 plot.initGraph(1, 6);
162
163 // For the first graphic :
164 // - along the x axis the expected values are between 0 and 200 and
165 // the step is 1
166 // - along the y axis the expected values are between -1.2 and 1.2 and the
167 // step is 0.1
168 plot.initRange(0, 0, 200, 1, -1.2, 1.2, 0.1);
169 plot.setTitle(0, "Joint behavior");
170 plot.initRange(1, 0, 200, 1, -0.01, 0.01, 0.05);
171 plot.setTitle(1, "Joint velocity");
172
173 // For the first graphic, set the curves legend
174 std::string legend;
175 for (unsigned int i = 0; i < 6; i++) {
176 legend = "q" + i + 1;
177 plot.setLegend(0, i, legend);
178 plot.setLegend(1, i, legend);
179 }
180 plot.setLegend(0, 6, "tQmin");
181 plot.setLegend(0, 7, "tQmax");
182 plot.setLegend(0, 8, "Qmin");
183 plot.setLegend(0, 9, "Qmax");
184
185 // Set the curves color
186 plot.setColor(0, 0, vpColor::red);
187 plot.setColor(0, 1, vpColor::green);
188 plot.setColor(0, 2, vpColor::blue);
189 plot.setColor(0, 3, vpColor::orange);
190 plot.setColor(0, 4, vpColor(0, 128, 0));
191 plot.setColor(0, 5, vpColor::cyan);
192 for (unsigned int i = 6; i < 10; i++)
193 plot.setColor(0, i, vpColor::black); // for Q and tQ [min,max]
194 // Set the curves color
195
196 plot.setColor(1, 0, vpColor::red);
197 plot.setColor(1, 1, vpColor::green);
198 plot.setColor(1, 2, vpColor::blue);
199 plot.setColor(1, 3, vpColor::orange);
200 plot.setColor(1, 4, vpColor(0, 128, 0));
201 plot.setColor(1, 5, vpColor::cyan);
202 vpDot2 dot;
203
204 std::cout << "Click on a dot..." << std::endl;
205 dot.initTracking(I);
206 vpImagePoint cog = dot.getCog();
209
211 // Update camera parameters
212 robot.getCameraParameters(cam, I);
213
214 // sets the current position of the visual feature
216 vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
217
218 p.set_Z(1);
219 // sets the desired position of the visual feature
221 pd.buildFrom(0, 0, 1);
222
223 // Define the task
224 // - we want an eye-in-hand control law
225 // - articular velocity are computed
228
230 robot.get_cVe(cVe);
231 std::cout << cVe << std::endl;
232 task.set_cVe(cVe);
233
234 // - Set the Jacobian (expressed in the end-effector frame)") ;
235 vpMatrix eJe;
236 robot.get_eJe(eJe);
237 task.set_eJe(eJe);
238
239 // - we want to see a point on a point..") ;
240 std::cout << std::endl;
241 task.addFeature(p, pd);
242
243 // - set the gain
244 double lambda = 0.8;
245 // set to -1 to suppress the lambda used in the
246 // vpServo::computeControlLaw()
247 task.setLambda(-1);
248
249 // Display task information " ) ;
250 task.print();
251
253
254 int iter = 0;
255 double t_1 = vpTime::measureTimeMs();
256
257 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
258 for (;;) {
259 iter++;
260
261 double t_0 = vpTime::measureTimeMs(); // t_0: current time
262
263 // Update loop time in second
264 double Tv = (double)(t_0 - t_1) / 1000.0;
265 std::cout << "Tv: " << Tv << std::endl;
266
267 // Update time for next iteration
268 t_1 = t_0;
269
270 // Acquire a new image from the camera
271 dc1394video_frame_t *frame = g.dequeue(I);
272
273 // Display this image
275
276 // Achieve the tracking of the dot in the image
277 dot.track(I);
278 cog = dot.getCog();
279
280 // Display a green cross at the center of gravity position in the image
282
283 // Get the measured joint positions of the robot
284 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
285
286 // Update the point feature from the dot location
287 vpFeatureBuilder::create(p, cam, dot);
288
289 // Get the jacobian of the robot
290 robot.get_eJe(eJe);
291 // Update this jacobian in the task structure. It will be used to
292 // compute the velocity skew (as an articular velocity) qdot = -lambda *
293 // L^+ * cVe * eJe * (s-s*)
294 task.set_eJe(eJe);
295
296 vpColVector prim_task;
297 vpColVector e2(6);
298 // Compute the visual servoing skew vector
299 prim_task = task.computeControlLaw();
300
301 vpColVector qpre(6);
302
303 qpre = q;
304 qpre += -lambda * prim_task * (4 * Tloop);
305
306 // Identify the joints near the limits
307 vpColVector pb(6);
308 pb = 0;
309 unsigned int npb = 0;
310 for (unsigned int i = 0; i < 6; i++) {
311 if (q[i] < tQmin[i])
312 if (fabs(Qmin[i] - q[i]) > fabs(Qmin[i] - qpre[i])) {
313 pb[i] = 1;
314 npb++;
315 std::cout << "Joint " << i << " near limit " << std::endl;
316 }
317 if (q[i] > tQmax[i]) {
318 if (fabs(Qmax[i] - q[i]) > fabs(Qmax[i] - qpre[i])) {
319 pb[i] = 1;
320 npb++;
321 std::cout << "Joint " << i << " near limit " << std::endl;
322 }
323 }
324 }
325
326 vpColVector a0;
327 vpMatrix J1 = task.getTaskJacobian();
328 vpMatrix kernelJ1;
329 J1.kernel(kernelJ1);
330
331 unsigned int dimKernelL = kernelJ1.getCols();
332 if (npb != 0) {
333 // Build linear system a0*E = S
334 vpMatrix E(npb, dimKernelL);
335 vpColVector S(npb);
336
337 unsigned int k = 0;
338
339 for (unsigned int j = 0; j < 6; j++) // j is the joint
340 if (std::fabs(pb[j] - 1) <= std::numeric_limits<double>::epsilon()) {
341 for (unsigned int i = 0; i < dimKernelL; i++)
342 E[k][i] = kernelJ1[j][i];
343
344 S[k] = -prim_task[j];
345 k++;
346 }
347 vpMatrix Ep;
348 // vpTRACE("nbp %d", npb);
349 Ep = E.t() * (E * E.t()).pseudoInverse();
350 a0 = Ep * S;
351
352 e2 = (kernelJ1 * a0);
353 // cout << "e2 " << e2.t() ;
354 } else {
355 e2 = 0;
356 }
357 // std::cout << "e2: " << e2.t() << std::endl;
358
359 vpColVector v;
360 v = -lambda * (prim_task + e2);
361
362 // Display the current and desired feature points in the image display
363 vpServoDisplay::display(task, cam, I);
364
365 // Apply the computed joint velocities to the robot
367
368 {
369 // Add the material to plot curves
370
371 // q normalized between (entre -1 et 1)
372 for (unsigned int i = 0; i < 6; i++) {
373 data[i] = (q[i] - Qmiddle[i]);
374 data[i] /= (Qmax[i] - Qmin[i]);
375 data[i] *= 2;
376 }
377 unsigned int joint = 2;
378 data[6] = 2 * (tQmin[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
379 data[7] = 2 * (tQmax[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
380 data[8] = -1;
381 data[9] = 1;
382
383 plot.plot(0, iter, data); // plot q, Qmin, Qmax, tQmin, tQmax
384 plot.plot(1, iter, v); // plot joint velocities applied to the robot
385 }
386
388
389 // Synchronize the loop with the image frame rate
390 vpTime::wait(t_0, 1000. * Tloop);
391 // Release the ring buffer used for the last image to start a new acq
392 g.enqueue(frame);
393 }
394
395 // Display task information
396 task.print();
397 return EXIT_SUCCESS;
398 } catch (const vpException &e) {
399 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
400 return EXIT_FAILURE;
401 }
402}
403
404#else
405int main()
406{
407 std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
408 return EXIT_SUCCESS;
409}
410#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
unsigned int getCols() const
Definition vpArray2D.h:280
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
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
vpMatrix t() const
Definition vpMatrix.cpp:461
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
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpMatrix getTaskJacobian() const
Definition vpServo.cpp:1765
@ 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
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()