Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6Point2DArtVelocity.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
57#include <fstream>
58#include <iostream>
59#include <sstream>
60#include <stdio.h>
61#include <stdlib.h>
62#include <visp3/core/vpConfig.h>
63#include <visp3/core/vpDebug.h> // Debug trace
64#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
65
66#include <visp3/core/vpDisplay.h>
67#include <visp3/core/vpImage.h>
68#include <visp3/core/vpImagePoint.h>
69#include <visp3/gui/vpDisplayGTK.h>
70#include <visp3/gui/vpDisplayOpenCV.h>
71#include <visp3/gui/vpDisplayX.h>
72#include <visp3/sensor/vp1394TwoGrabber.h>
73
74#include <visp3/core/vpHomogeneousMatrix.h>
75#include <visp3/core/vpIoTools.h>
76#include <visp3/core/vpMath.h>
77#include <visp3/core/vpPoint.h>
78#include <visp3/robot/vpRobotAfma6.h>
79#include <visp3/visual_features/vpFeatureBuilder.h>
80#include <visp3/visual_features/vpFeaturePoint.h>
81#include <visp3/vs/vpServo.h>
82
83// Exception
84#include <visp3/core/vpException.h>
85#include <visp3/vs/vpServoDisplay.h>
86
87#include <visp3/blob/vpDot.h>
88
89int main()
90{
91 // Log file creation in /tmp/$USERNAME/log.dat
92 // This file contains by line:
93 // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
94 // - the 6 mesured joint velocities (m/s, rad/s)
95 // - the 6 mesured joint positions (m, rad)
96 // - the 2 values of s - s*
97 std::string username;
98 // Get the user login name
99 vpIoTools::getUserName(username);
100
101 // Create a log filename to save velocities...
102 std::string logdirname;
103 logdirname = "/tmp/" + username;
104
105 // Test if the output path exist. If no try to create it
106 if (vpIoTools::checkDirectory(logdirname) == false) {
107 try {
108 // Create the dirname
109 vpIoTools::makeDirectory(logdirname);
110 } catch (...) {
111 std::cerr << std::endl << "ERROR:" << std::endl;
112 std::cerr << " Cannot create " << logdirname << std::endl;
113 return EXIT_FAILURE;
114 }
115 }
116 std::string logfilename;
117 logfilename = logdirname + "/log.dat";
118
119 // Open the log file name
120 std::ofstream flog(logfilename.c_str());
121
122 try {
123 vpServo task;
124
126
130 g.open(I);
131
132 g.acquire(I);
133
134#ifdef VISP_HAVE_X11
135 vpDisplayX display(I, 100, 100, "Current image");
136#elif defined(HAVE_OPENCV_HIGHGUI)
137 vpDisplayOpenCV display(I, 100, 100, "Current image");
138#elif defined(VISP_HAVE_GTK)
139 vpDisplayGTK display(I, 100, 100, "Current image");
140#endif
141
144 // exit(1) ;
145
146 std::cout << std::endl;
147 std::cout << "-------------------------------------------------------" << std::endl;
148 std::cout << " Test program for vpServo " << std::endl;
149 std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl;
150 std::cout << " Use of the Afma6 robot " << std::endl;
151 std::cout << " task : servo a point " << std::endl;
152 std::cout << "-------------------------------------------------------" << std::endl;
153 std::cout << std::endl;
154
155 vpDot dot;
156 vpImagePoint cog;
157
158 std::cout << "Click on a dot..." << std::endl;
159 dot.initTracking(I);
160 // Get the dot cog
161 cog = dot.getCog();
164
165 vpRobotAfma6 robot;
166
168 // Update camera parameters
169 robot.getCameraParameters(cam, I);
170
171 vpTRACE("sets the current position of the visual feature ");
173 vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
174
175 p.set_Z(1);
176 vpTRACE("sets the desired position of the visual feature ");
178 pd.buildFrom(0, 0, 1);
179
180 vpTRACE("define the task");
181 vpTRACE("\t we want an eye-in-hand control law");
182 vpTRACE("\t articular velocity are computed");
185
186 vpTRACE("Set the position of the end-effector frame in the camera frame");
188 // robot.get_cMe(cMe) ;
189
191 robot.get_cVe(cVe);
192 std::cout << cVe << std::endl;
193 task.set_cVe(cVe);
194
195 // vpDisplay::getClick(I) ;
196 vpTRACE("Set the Jacobian (expressed in the end-effector frame)");
197 vpMatrix eJe;
198 robot.get_eJe(eJe);
199 task.set_eJe(eJe);
200
201 vpTRACE("\t we want to see a point on a point..");
202 std::cout << std::endl;
203 task.addFeature(p, pd);
204
205 vpTRACE("\t set the gain");
206 task.setLambda(0.8);
207
208 vpTRACE("Display task information ");
209 task.print();
210
212
213 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
214 for (;;) {
215 // Acquire a new image from the camera
216 g.acquire(I);
217
218 // Display this image
220
221 // Achieve the tracking of the dot in the image
222 dot.track(I);
223
224 // Get the dot cog
225 cog = dot.getCog();
226
227 // Display a green cross at the center of gravity position in the image
229
230 // Update the point feature from the dot location
231 vpFeatureBuilder::create(p, cam, dot);
232
233 // Get the jacobian of the robot
234 robot.get_eJe(eJe);
235 // Update this jacobian in the task structure. It will be used to
236 // compute the velocity skew (as an articular velocity) qdot = -lambda *
237 // L^+ * cVe * eJe * (s-s*)
238 task.set_eJe(eJe);
239
240 // std::cout << (vpMatrix)cVe*eJe << std::endl ;
241
242 vpColVector v;
243 // Compute the visual servoing skew vector
244 v = task.computeControlLaw();
245
246 // Display the current and desired feature points in the image display
247 vpServoDisplay::display(task, cam, I);
248
249 // Apply the computed joint velocities to the robot
251
252 // Save velocities applied to the robot in the log file
253 // v[0], v[1], v[2] correspond to joint translation velocities in m/s
254 // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s
255 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
256
257 // Get the measured joint velocities of the robot
258 vpColVector qvel;
260 // Save measured joint velocities of the robot in the log file:
261 // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
262 // velocities in m/s
263 // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
264 // velocities in rad/s
265 flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
266
267 // Get the measured joint positions of the robot
268 vpColVector q;
269 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
270 // Save measured joint positions of the robot in the log file
271 // - q[0], q[1], q[2] correspond to measured joint translation
272 // positions in m
273 // - q[3], q[4], q[5] correspond to measured joint rotation
274 // positions in rad
275 flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
276
277 // Save feature error (s-s*) for the feature point. For this feature
278 // point, we have 2 errors (along x and y axis). This error is
279 // expressed in meters in the camera frame
280 flog << (task.getError()).t() << std::endl;
282
283 // vpTRACE("\t\t || s - s* || = %f ", ( task.getError()
284 // ).sumSquare()) ;
285 }
286
287 flog.close(); // Close the log file
288
289 vpTRACE("Display task information ");
290 task.print();
291 return EXIT_SUCCESS;
292 } catch (const vpException &e) {
293 flog.close(); // Close the log file
294 std::cout << "Test failed with exception: " << e << std::endl;
295 return EXIT_FAILURE;
296 }
297}
298
299#else
300int main()
301{
302 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
303 return EXIT_SUCCESS;
304}
305#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
void setVideoMode(vp1394TwoVideoModeType videomode)
void setFramerate(vp1394TwoFramerateType fps)
void open(vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
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 dot (connected pixels with same gray level) on a vpImage.
Definition vpDot.h:112
void initTracking(const vpImage< unsigned char > &I)
Definition vpDot.cpp:617
vpImagePoint getCog() const
Definition vpDot.h:243
void track(const vpImage< unsigned char > &I)
Definition vpDot.cpp:757
error that can be emitted by ViSP classes.
Definition vpException.h:59
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
Control of Irisa's gantry robot named Afma6.
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