Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoViper850Point2DArtVelocity-jointAvoidance-large.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_2) && 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 Qmiddle(6);
109 vpColVector data(12);
110
111 Qmiddle = (jointMin + jointMax) / 2.;
112 // double rho1 = 0.1 ;
113
114 double rho = 0.1;
115 double rho1 = 0.3;
116
117 vpColVector q(6);
118
119 // Create a window with two graphics
120 // - first graphic to plot q(t), Qmin, Qmax, Ql0min, Ql1min, Ql0max and
121 // Ql1max
122 vpPlot plot(2);
123
124 // The first graphic contains 12 data to plot: q(t), Low Limits, Upper
125 // Limits, ql0min, ql1min, ql0max and ql1max
126 plot.initGraph(0, 12);
127 // The second graphic contains the values of the secondaty task velocities
128 plot.initGraph(1, 6);
129
130 // For the first graphic :
131 // - along the x axis the expected values are between 0 and 200
132 // - along the y axis the expected values are between -1.2 and 1.2
133 plot.initRange(0, 0., 200., -1.2, 1.2);
134 plot.setTitle(0, "Joint behavior");
135
136 // For the second graphic :
137 plot.setTitle(1, "Q secondary task");
138
139 // For the first and second graphic, set the curves legend
140 std::string legend;
141 for (unsigned int i = 0; i < 6; i++) {
142 legend = "q" + i + 1;
143 plot.setLegend(0, i, legend);
144 plot.setLegend(1, i, legend);
145 }
146 plot.setLegend(0, 6, "Low Limit");
147 plot.setLegend(0, 7, "Upper Limit");
148 plot.setLegend(0, 8, "ql0 min");
149 plot.setLegend(0, 9, "ql0 max");
150 plot.setLegend(0, 10, "ql1 min");
151 plot.setLegend(0, 11, "ql1 max");
152
153 // Set the curves color
154 plot.setColor(0, 0, vpColor::red);
155 plot.setColor(0, 1, vpColor::green);
156 plot.setColor(0, 2, vpColor::blue);
157 plot.setColor(0, 3, vpColor::orange);
158 plot.setColor(0, 4, vpColor(0, 128, 0));
159 plot.setColor(0, 5, vpColor::cyan);
160 for (unsigned int i = 6; i < 12; i++)
161 plot.setColor(0, i, vpColor::black); // for Q and tQ [min,max]
162
163 vpColVector sec_task(6);
164
165 vpDot2 dot;
166
167 std::cout << "Click on a dot..." << std::endl;
168 dot.initTracking(I);
169 vpImagePoint cog = dot.getCog();
172
174 // Update camera parameters
175 robot.getCameraParameters(cam, I);
176
177 // sets the current position of the visual feature
179 vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
180
181 p.set_Z(1);
182 // sets the desired position of the visual feature
184 pd.buildFrom(0, 0, 1);
185
186 // Define the task
187 // - we want an eye-in-hand control law
188 // - articular velocity are computed
191
193 robot.get_cVe(cVe);
194 std::cout << cVe << std::endl;
195 task.set_cVe(cVe);
196
197 // - Set the Jacobian (expressed in the end-effector frame)") ;
198 vpMatrix eJe;
199 robot.get_eJe(eJe);
200 task.set_eJe(eJe);
201
202 // - we want to see a point on a point..") ;
203 std::cout << std::endl;
204 task.addFeature(p, pd);
205
206 // - set the gain
207 task.setLambda(0.8);
208
209 // Display task information " ) ;
210 task.print();
211
213
214 int iter = 0;
215 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
216 for (;;) {
217 iter++;
218 // Acquire a new image from the camera
219 g.acquire(I);
220
221 // Display this image
223
224 // Achieve the tracking of the dot in the image
225 dot.track(I);
226 cog = dot.getCog();
227
228 // Display a green cross at the center of gravity position in the image
230
231 // Get the measured joint positions of the robot
232 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
233
234 // Update the point feature from the dot location
235 vpFeatureBuilder::create(p, cam, dot);
236
237 // Get the jacobian of the robot
238 robot.get_eJe(eJe);
239 // Update this jacobian in the task structure. It will be used to
240 // compute the velocity skew (as an articular velocity) qdot = -lambda *
241 // L^+ * cVe * eJe * (s-s*)
242 task.set_eJe(eJe);
243
244 vpColVector prim_task;
245 // Compute the visual servoing skew vector
246 prim_task = task.computeControlLaw();
247
248 // Compute the secondary task for the joint limit avoidance
249 sec_task = task.secondaryTaskJointLimitAvoidance(q, prim_task, jointMin, jointMax, rho, rho1);
250
251 vpColVector v;
252 v = prim_task + sec_task;
253
254 // Display the current and desired feature points in the image display
255 vpServoDisplay::display(task, cam, I);
256
257 // Apply the computed joint velocities to the robot
259
260 {
261 // Add the material to plot curves
262
263 // q normalized between (entre -1 et 1)
264 for (unsigned int i = 0; i < 6; i++) {
265 data[i] = (q[i] - Qmiddle[i]);
266 data[i] /= (jointMax[i] - jointMin[i]);
267 data[i] *= 2;
268 }
269
270 data[6] = -1.0;
271 data[7] = 1.0;
272
273 unsigned int joint = 2;
274 double tQmin_l0 = jointMin[joint] + rho * (jointMax[joint] - jointMin[joint]);
275 double tQmax_l0 = jointMax[joint] - rho * (jointMax[joint] - jointMin[joint]);
276
277 double tQmin_l1 = tQmin_l0 - rho * rho1 * (jointMax[joint] - jointMin[joint]);
278 double tQmax_l1 = tQmax_l0 + rho * rho1 * (jointMax[joint] - jointMin[joint]);
279
280 data[8] = 2 * (tQmin_l0 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
281 data[9] = 2 * (tQmax_l0 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
282 data[10] = 2 * (tQmin_l1 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
283 data[11] = 2 * (tQmax_l1 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
284 plot.plot(0, iter, data); // plot q(t), Low Limits, Upper Limits,
285 // ql0min, ql1min, ql0max and ql1max
286 plot.plot(1, iter, sec_task); // plot secondary task velocities
287 }
288
290 }
291
292 // Display task information
293 task.print();
294 return EXIT_SUCCESS;
295 } catch (const vpException &e) {
296 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
297 return EXIT_FAILURE;
298 }
299}
300
301#else
302int main()
303{
304 std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
305 return EXIT_SUCCESS;
306}
307#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
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
vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &jointMin, const vpColVector &jointMax, const double &rho=0.1, const double &rho1=0.3, const double &lambda_tune=0.7)
Definition vpServo.cpp:1641
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
@ 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