Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma4Point2DCamVelocity.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 the camera frame
35 *
36*****************************************************************************/
37
58#include <stdlib.h>
59#include <visp3/core/vpConfig.h>
60#include <visp3/core/vpDebug.h> // Debug trace
61#if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
62
63#include <visp3/core/vpDisplay.h>
64#include <visp3/core/vpImage.h>
65#include <visp3/core/vpImagePoint.h>
66#include <visp3/gui/vpDisplayGTK.h>
67#include <visp3/gui/vpDisplayOpenCV.h>
68#include <visp3/gui/vpDisplayX.h>
69#include <visp3/sensor/vp1394TwoGrabber.h>
70
71#include <visp3/core/vpHomogeneousMatrix.h>
72#include <visp3/core/vpIoTools.h>
73#include <visp3/core/vpMath.h>
74#include <visp3/core/vpPoint.h>
75#include <visp3/robot/vpRobotAfma4.h>
76#include <visp3/visual_features/vpFeatureBuilder.h>
77#include <visp3/visual_features/vpFeaturePoint.h>
78#include <visp3/vs/vpServo.h>
79
80// Exception
81#include <visp3/core/vpException.h>
82#include <visp3/vs/vpServoDisplay.h>
83
84#include <visp3/blob/vpDot.h>
85
86int main()
87{
88 try {
89 // Log file creation in /tmp/$USERNAME/log.dat
90 // This file contains by line:
91 // - the 6 computed cam velocities (m/s, rad/s) to achieve the task
92 // - the 6 mesured joint velocities (m/s, rad/s)
93 // - the 6 mesured joint positions (m, rad)
94 // - the 2 values of s - s*
95 std::string username;
96 // Get the user login name
97 vpIoTools::getUserName(username);
98
99 // Create a log filename to save velocities...
100 std::string logdirname;
101 logdirname = "/tmp/" + username;
102
103 // Test if the output path exist. If no try to create it
104 if (vpIoTools::checkDirectory(logdirname) == false) {
105 try {
106 // Create the dirname
107 vpIoTools::makeDirectory(logdirname);
108 } catch (...) {
109 std::cerr << std::endl << "ERROR:" << std::endl;
110 std::cerr << " Cannot create " << logdirname << std::endl;
111 return EXIT_FAILURE;
112 }
113 }
114 std::string logfilename;
115 logfilename = logdirname + "/log.dat";
116
117 // Open the log file name
118 std::ofstream flog(logfilename.c_str());
119
120 vpRobotAfma4 robot;
121 vpServo task;
122
124
125 vp1394TwoGrabber g(false);
127 g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
128 g.open(I);
129
130 g.acquire(I);
131
132#ifdef VISP_HAVE_X11
133 vpDisplayX display(I, 100, 100, "Current image");
134#elif defined(HAVE_OPENCV_HIGHGUI)
135 vpDisplayOpenCV display(I, 100, 100, "Current image");
136#elif defined(VISP_HAVE_GTK)
137 vpDisplayGTK display(I, 100, 100, "Current image");
138#endif
139
142
143 std::cout << std::endl;
144 std::cout << "-------------------------------------------------------" << std::endl;
145 std::cout << " Test program for vpServo " << std::endl;
146 std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
147 std::cout << " Simulation " << std::endl;
148 std::cout << " task : servo a point " << std::endl;
149 std::cout << "-------------------------------------------------------" << std::endl;
150 std::cout << std::endl;
151
152 vpDot dot;
153
154 std::cout << "Click on a dot..." << std::endl;
155 dot.initTracking(I);
156
157 // Get the cog of the dot
158 vpImagePoint cog = dot.getCog();
159
162
164
165 vpTRACE("sets the current position of the visual feature ");
167 vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
168
169 vpTRACE("sets the desired position of the visual feature ");
171 pd.buildFrom(0, 0, 1);
172
173 vpTRACE("define the task");
174 vpTRACE("\t we want an eye-in-hand control law");
175 vpTRACE("\t robot is controlled in the camera frame");
177
178 vpTRACE("\t we want to see a point on a point..");
179 std::cout << std::endl;
180 task.addFeature(p, pd);
181
182 vpTRACE("\t set the gain");
183 task.setLambda(0.8);
184
185 vpTRACE("Display task information ");
186 task.print();
187
189
190 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
191 for (;;) {
192 // Acquire a new image from the camera
193 g.acquire(I);
194
195 // Display this image
197
198 // Achieve the tracking of the dot in the image
199 dot.track(I);
200
201 // Get the cog of the dot
202 vpImagePoint cog = dot.getCog();
203
204 // Display a green cross at the center of gravity position in the image
206
207 // Update the point feature from the dot location
208 vpFeatureBuilder::create(p, cam, dot);
209
210 vpColVector v;
211 // Compute the visual servoing skew vector
212 v = task.computeControlLaw();
213
214 // Display the current and desired feature points in the image display
215 vpServoDisplay::display(task, cam, I);
216
217 // Apply the computed joint velocities to the robot
219
220 // Save velocities applied to the robot in the log file
221 // v[0], v[1], v[2] correspond to camera translation velocities in m/s
222 // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
223 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
224
225 // Get the measured joint velocities of the robot
226 vpColVector qvel;
228 // Save measured joint velocities of the robot in the log file:
229 // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
230 // velocities in m/s
231 // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
232 // velocities in rad/s
233 flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
234
235 // Get the measured joint positions of the robot
236 vpColVector q;
237 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
238 // Save measured joint positions of the robot in the log file
239 // - q[0], q[1], q[2] correspond to measured joint translation
240 // positions in m
241 // - q[3], q[4], q[5] correspond to measured joint rotation
242 // positions in rad
243 flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
244
245 // Save feature error (s-s*) for the feature point. For this feature
246 // point, we have 2 errors (along x and y axis). This error is
247 // expressed in meters in the camera frame
248 flog << task.getError() << std::endl;
249
250 // Flush the display
252 }
253
254 flog.close(); // Close the log file
255
256 // Display task information
257 task.print();
258
259 return EXIT_SUCCESS;
260 } catch (const vpException &e) {
261 std::cout << "Catch a ViSP exception: " << e << std::endl;
262 return EXIT_FAILURE;
263 }
264}
265
266#else
267int main()
268{
269 std::cout << "You do not have an afma4 robot connected to your computer..." << std::endl;
270 return EXIT_SUCCESS;
271}
272#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.
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)
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)
Control of Irisa's cylindrical robot named Afma4.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ CAMERA_FRAME
Definition vpRobot.h:80
@ 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)
@ EYEINHAND_CAMERA
Definition vpServo.h:151
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 setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector getError() const
Definition vpServo.h:276
vpColVector computeControlLaw()
Definition vpServo.cpp:930
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
#define vpTRACE
Definition vpDebug.h:411