Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoViper850Point2DArtVelocity.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 <fstream>
51#include <iostream>
52#include <sstream>
53#include <stdio.h>
54#include <stdlib.h>
55
56#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
57
58#include <visp3/core/vpDisplay.h>
59#include <visp3/core/vpHomogeneousMatrix.h>
60#include <visp3/core/vpImage.h>
61#include <visp3/core/vpIoTools.h>
62#include <visp3/core/vpMath.h>
63#include <visp3/core/vpPoint.h>
64#include <visp3/gui/vpDisplayGTK.h>
65#include <visp3/gui/vpDisplayOpenCV.h>
66#include <visp3/gui/vpDisplayX.h>
67#include <visp3/robot/vpRobotViper850.h>
68#include <visp3/sensor/vp1394TwoGrabber.h>
69#include <visp3/visual_features/vpFeatureBuilder.h>
70#include <visp3/visual_features/vpFeaturePoint.h>
71#include <visp3/vs/vpServo.h>
72
73// Exception
74#include <visp3/core/vpException.h>
75#include <visp3/vs/vpServoDisplay.h>
76
77#include <visp3/blob/vpDot2.h>
78
79int main()
80{
81 // Log file creation in /tmp/$USERNAME/log.dat
82 // This file contains by line:
83 // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
84 // - the 6 mesured joint velocities (m/s, rad/s)
85 // - the 6 mesured joint positions (m, rad)
86 // - the 2 values of s - s*
87 std::string username;
88 // Get the user login name
89 vpIoTools::getUserName(username);
90
91 // Create a log filename to save velocities...
92 std::string logdirname;
93 logdirname = "/tmp/" + username;
94
95 // Test if the output path exist. If no try to create it
96 if (vpIoTools::checkDirectory(logdirname) == false) {
97 try {
98 // Create the dirname
99 vpIoTools::makeDirectory(logdirname);
100 } catch (...) {
101 std::cerr << std::endl << "ERROR:" << std::endl;
102 std::cerr << " Cannot create " << logdirname << std::endl;
103 return EXIT_FAILURE;
104 }
105 }
106 std::string logfilename;
107 logfilename = logdirname + "/log.dat";
108
109 // Open the log file name
110 std::ofstream flog(logfilename.c_str());
111
112 try {
113 vpRobotViper850 robot;
114
115 vpServo task;
116
118
119 bool reset = false;
120 vp1394TwoGrabber g(reset);
122 g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
123 g.open(I);
124
125 g.acquire(I);
126
127#ifdef VISP_HAVE_X11
128 vpDisplayX display(I, 800, 100, "Current image");
129#elif defined(HAVE_OPENCV_HIGHGUI)
130 vpDisplayOpenCV display(I, 800, 100, "Current image");
131#elif defined(VISP_HAVE_GTK)
132 vpDisplayGTK display(I, 800, 100, "Current image");
133#endif
134
137 // exit(1) ;
138
139 std::cout << std::endl;
140 std::cout << "-------------------------------------------------------" << std::endl;
141 std::cout << " Test program for vpServo " << std::endl;
142 std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl;
143 std::cout << " Use of the Afma6 robot " << std::endl;
144 std::cout << " task : servo a point " << std::endl;
145 std::cout << "-------------------------------------------------------" << std::endl;
146 std::cout << std::endl;
147
148 vpDot2 dot;
149
150 std::cout << "Click on a dot..." << std::endl;
151 dot.initTracking(I);
152 vpImagePoint cog = dot.getCog();
155
157 // Update camera parameters
158 robot.getCameraParameters(cam, I);
159
160 vpTRACE("sets the current position of the visual feature ");
162 vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
163
164 p.set_Z(1);
165 vpTRACE("sets the desired position of the visual feature ");
167 pd.buildFrom(0, 0, 1);
168
169 vpTRACE("define the task");
170 vpTRACE("\t we want an eye-in-hand control law");
171 vpTRACE("\t articular velocity are computed");
174
175 vpTRACE("Set the position of the end-effector frame in the camera frame");
177 // robot.get_cMe(cMe) ;
178
180 robot.get_cVe(cVe);
181 std::cout << cVe << std::endl;
182 task.set_cVe(cVe);
183
184 // vpDisplay::getClick(I) ;
185 vpTRACE("Set the Jacobian (expressed in the end-effector frame)");
186 vpMatrix eJe;
187 robot.get_eJe(eJe);
188 task.set_eJe(eJe);
189
190 vpTRACE("\t we want to see a point on a point..");
191 std::cout << std::endl;
192 task.addFeature(p, pd);
193
194 vpTRACE("\t set the gain");
195 task.setLambda(0.8);
196
197 vpTRACE("Display task information ");
198 task.print();
199
201
202 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
203 for (;;) {
204 // Acquire a new image from the camera
205 g.acquire(I);
206
207 // Display this image
209
210 // Achieve the tracking of the dot in the image
211 dot.track(I);
212 cog = dot.getCog();
213
214 // Display a green cross at the center of gravity position in the image
216
217 // Update the point feature from the dot location
218 vpFeatureBuilder::create(p, cam, dot);
219
220 // Get the jacobian of the robot
221 robot.get_eJe(eJe);
222 // Update this jacobian in the task structure. It will be used to
223 // compute the velocity skew (as an articular velocity) qdot = -lambda *
224 // L^+ * cVe * eJe * (s-s*)
225 task.set_eJe(eJe);
226
227 // std::cout << (vpMatrix)cVe*eJe << std::endl ;
228
229 vpColVector v;
230 // Compute the visual servoing skew vector
231 v = task.computeControlLaw();
232
233 // Display the current and desired feature points in the image display
234 vpServoDisplay::display(task, cam, I);
235
236 // Apply the computed joint velocities to the robot
238
239 // Save velocities applied to the robot in the log file
240 // v[0], v[1], v[2] correspond to joint translation velocities in m/s
241 // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s
242 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
243
244 // Get the measured joint velocities of the robot
245 vpColVector qvel;
247 // Save measured joint velocities of the robot in the log file:
248 // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
249 // velocities in m/s
250 // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
251 // velocities in rad/s
252 flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
253
254 // Get the measured joint positions of the robot
255 vpColVector q;
256 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
257 // Save measured joint positions of the robot in the log file
258 // - q[0], q[1], q[2] correspond to measured joint translation
259 // positions in m
260 // - q[3], q[4], q[5] correspond to measured joint rotation
261 // positions in rad
262 flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
263
264 // Save feature error (s-s*) for the feature point. For this feature
265 // point, we have 2 errors (along x and y axis). This error is
266 // expressed in meters in the camera frame
267 flog << (task.getError()).t() << std::endl; // s-s* for point
268
270
271 // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<
272 // std::endl;
273 }
274
275 flog.close(); // Close the log file
276
277 std::cout << "Display task information: " << std::endl;
278 task.print();
279 return EXIT_SUCCESS;
280 } catch (const vpException &e) {
281 flog.close(); // Close the log file
282 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
283 return EXIT_FAILURE;
284 }
285}
286
287#else
288int main()
289{
290 std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
291 return EXIT_SUCCESS;
292}
293#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.
vpRowVector t() const
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)
Implementation of an homogeneous matrix and operations on such kind of matrices.
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 bool checkDirectory(const std::string &dirname)
static std::string getUserName()
static void makeDirectory(const std::string &dirname)
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
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
vpColVector getError() const
Definition vpServo.h:276
@ 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
#define vpTRACE
Definition vpDebug.h:411