Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6FourPoints2DCamVelocityLs_des.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
60#include <stdlib.h>
61#include <visp3/core/vpConfig.h>
62#include <visp3/core/vpDebug.h> // Debug trace
63#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
64
65#include <visp3/core/vpDisplay.h>
66#include <visp3/core/vpImage.h>
67#include <visp3/core/vpImagePoint.h>
68#include <visp3/gui/vpDisplayGTK.h>
69#include <visp3/gui/vpDisplayOpenCV.h>
70#include <visp3/gui/vpDisplayX.h>
71#include <visp3/sensor/vp1394TwoGrabber.h>
72
73#include <visp3/blob/vpDot.h>
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/core/vpRotationMatrix.h>
79#include <visp3/core/vpRxyzVector.h>
80#include <visp3/core/vpTranslationVector.h>
81#include <visp3/robot/vpRobotAfma6.h>
82#include <visp3/visual_features/vpFeatureBuilder.h>
83#include <visp3/visual_features/vpFeaturePoint.h>
84#include <visp3/vs/vpServo.h>
85#include <visp3/vs/vpServoDisplay.h>
86
87// Exception
88#include <visp3/core/vpException.h>
89
90#define L 0.05 // to deal with a 10cm by 10cm square
91
92int main()
93{
94 // Log file creation in /tmp/$USERNAME/log.dat
95 // This file contains by line:
96 // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
97 // - the 6 mesured camera velocities (m/s, rad/s)
98 // - the 6 mesured joint positions (m, rad)
99 // - the 8 values of s - s*
100 std::string username;
101 // Get the user login name
102 vpIoTools::getUserName(username);
103
104 // Create a log filename to save velocities...
105 std::string logdirname;
106 logdirname = "/tmp/" + username;
107
108 // Test if the output path exist. If no try to create it
109 if (vpIoTools::checkDirectory(logdirname) == false) {
110 try {
111 // Create the dirname
112 vpIoTools::makeDirectory(logdirname);
113 } catch (...) {
114 std::cerr << std::endl << "ERROR:" << std::endl;
115 std::cerr << " Cannot create " << logdirname << std::endl;
116 return EXIT_FAILURE;
117 }
118 }
119 std::string logfilename;
120 logfilename = logdirname + "/log.dat";
121
122 // Open the log file name
123 std::ofstream flog(logfilename.c_str());
124
125 try {
126 vpServo task;
127
129 int i;
130
134 g.open(I);
135
136#ifdef VISP_HAVE_X11
137 vpDisplayX display(I, 100, 100, "Current image");
138#elif defined(HAVE_OPENCV_HIGHGUI)
139 vpDisplayOpenCV display(I, 100, 100, "Current image");
140#elif defined(VISP_HAVE_GTK)
141 vpDisplayGTK display(I, 100, 100, "Current image");
142#endif
143
144 g.acquire(I);
145
148
149 std::cout << std::endl;
150 std::cout << "-------------------------------------------------------" << std::endl;
151 std::cout << " Test program for vpServo " << std::endl;
152 std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
153 std::cout << " Use of the Afma6 robot " << std::endl;
154 std::cout << " Interaction matrix computed with the desired features " << std::endl;
155
156 std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl;
157 std::cout << "-------------------------------------------------------" << std::endl;
158 std::cout << std::endl;
159
160 vpDot2 dot[4];
161 vpImagePoint cog;
162
163 std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl;
164 for (i = 0; i < 4; i++) {
165 dot[i].initTracking(I);
166 cog = dot[i].getCog();
169 }
170
171 vpRobotAfma6 robot;
172
174
175 // Load the end-effector to camera frame transformation obtained
176 // using a camera intrinsic model with distortion
177 robot.init(vpAfma6::TOOL_CCMOP, projModel);
178
180 // Update camera parameters
181 robot.getCameraParameters(cam, I);
182
183 // Sets the current position of the visual feature
184 vpFeaturePoint p[4];
185 for (i = 0; i < 4; i++)
186 vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure
187
188 // Set the position of the square target in a frame which origin is
189 // centered in the middle of the square
190 vpPoint point[4];
191 point[0].setWorldCoordinates(-L, -L, 0);
192 point[1].setWorldCoordinates(L, -L, 0);
193 point[2].setWorldCoordinates(L, L, 0);
194 point[3].setWorldCoordinates(-L, L, 0);
195
196 // Initialise a desired pose to compute s*, the desired 2D point features
198 vpTranslationVector cto(0, 0, 0.7); // tz = 0.7 meter
200 vpMath::rad(0)); // No rotations
201 vpRotationMatrix cRo(cro); // Build the rotation matrix
202 cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
203
204 // sets the desired position of the 2D visual feature
205 vpFeaturePoint pd[4];
206 // Compute the desired position of the features from the desired pose
207 for (int i = 0; i < 4; i++) {
208 vpColVector cP, p;
209 point[i].changeFrame(cMo, cP);
210 point[i].projection(cP, p);
211
212 pd[i].set_x(p[0]);
213 pd[i].set_y(p[1]);
214 pd[i].set_Z(cP[2]);
215 }
216
217 // Define the task
218 // - we want an eye-in-hand control law
219 // - robot is controlled in the camera frame
220 // - Interaction matrix is computed with the desired visual features
223
224 // We want to see a point on a point
225 std::cout << std::endl;
226 for (i = 0; i < 4; i++)
227 task.addFeature(p[i], pd[i]);
228
229 // Set the proportional gain
230 task.setLambda(0.4);
231
232 // Display task information
233 task.print();
234
235 // Initialise the velocity control of the robot
237
238 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
239
240 for (;;) {
241 // Acquire a new image from the camera
242 g.acquire(I);
243
244 // Display this image
246
247 // For each point...
248 for (i = 0; i < 4; i++) {
249 // Achieve the tracking of the dot in the image
250 dot[i].track(I);
251 // Get the dot cog
252 cog = dot[i].getCog();
253 // Display a green cross at the center of gravity position in the
254 // image
256 }
257
258 // Printing on stdout concerning task information
259 // task.print() ;
260
261 // Update the point feature from the dot location
262 for (i = 0; i < 4; i++)
263 vpFeatureBuilder::create(p[i], cam, dot[i]);
264
265 vpColVector v;
266 // Compute the visual servoing skew vector
267 v = task.computeControlLaw();
268
269 // Display the current and desired feature points in the image display
270 vpServoDisplay::display(task, cam, I);
271
272 // Apply the computed camera velocities to the robot
274
275 // Save velocities applied to the robot in the log file
276 // v[0], v[1], v[2] correspond to camera translation velocities in m/s
277 // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
278 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
279
280 // Get the measured joint velocities of the robot
281 vpColVector qvel;
283 // Save measured camera velocities of the robot in the log file:
284 // - qvel[0], qvel[1], qvel[2] correspond to measured camera translation
285 // velocities in m/s
286 // - qvel[3], qvel[4], qvel[5] correspond to measured camera rotation
287 // velocities in rad/s
288 flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
289
290 // Get the measured joint positions of the robot
291 vpColVector q;
292 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
293 // Save measured joint positions of the robot in the log file
294 // - q[0], q[1], q[2] correspond to measured joint translation
295 // positions in m
296 // - q[3], q[4], q[5] correspond to measured joint rotation
297 // positions in rad
298 flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
299
300 // Save feature error (s-s*) for the 4 feature points. For each feature
301 // point, we have 2 errors (along x and y axis). This error is
302 // expressed in meters in the camera frame
303 flog << (task.getError()).t() << std::endl;
304
305 // Flush the display
307 }
308
309 flog.close(); // Close the log file
310
311 // Display task information
312 task.print();
313
314 return EXIT_SUCCESS;
315 } catch (const vpException &e) {
316 flog.close(); // Close the log file
317 std::cout << "Test failed with exception: " << e << std::endl;
318 return EXIT_FAILURE;
319 }
320}
321
322#else
323int main()
324{
325 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
326 return EXIT_SUCCESS;
327}
328
329#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)
@ TOOL_CCMOP
Definition vpAfma6.h:124
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
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
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 set_y(double y)
void set_x(double x)
void set_Z(double Z)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
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)
static double rad(double deg)
Definition vpMath.h:116
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void projection(const vpColVector &_cP, vpColVector &_p) const
Definition vpPoint.cpp:219
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition vpPoint.cpp:236
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
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)
@ 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
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
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_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
@ 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
Class that consider the case of a translation vector.