Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6FourPoints2DCamVelocityLs_cur.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/vision/vpPose.h>
83#include <visp3/visual_features/vpFeatureBuilder.h>
84#include <visp3/visual_features/vpFeaturePoint.h>
85#include <visp3/vs/vpServo.h>
86#include <visp3/vs/vpServoDisplay.h>
87
88// Exception
89#include <visp3/core/vpException.h>
90
91#define L 0.05 // to deal with a 10cm by 10cm square
92
114void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, vpCameraParameters cam, vpHomogeneousMatrix &cMo, bool init)
115{
117 vpPose pose;
118 vpImagePoint cog;
119 for (int i = 0; i < ndot; i++) {
120
121 double x = 0, y = 0;
122
123 cog = dot[i].getCog();
125 y); // pixel to meter conversion
126 // std::cout << "point cam: " << i << x << " " << y << std::endl;
127 point[i].set_x(x); // projection perspective p
128 point[i].set_y(y);
129 pose.addPoint(point[i]);
130 // std::cout << "point " << i << std::endl;
131 // point[i].print();
132 }
133
134 if (init == true) {
136 } else { // init = false; use of the previous pose to initialise VIRTUAL_VS
138 }
139}
140
141int main()
142{
143 // Log file creation in /tmp/$USERNAME/log.dat
144 // This file contains by line:
145 // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
146 // - the 6 mesured joint velocities (m/s, rad/s)
147 // - the 6 mesured joint positions (m, rad)
148 // - the 8 values of s - s*
149 // - the 6 values of the pose cMo (tx,ty,tz, rx,ry,rz) with translation
150 // in meters and rotations in radians
151 std::string username;
152 // Get the user login name
153 vpIoTools::getUserName(username);
154
155 // Create a log filename to save velocities...
156 std::string logdirname;
157 logdirname = "/tmp/" + username;
158
159 // Test if the output path exist. If no try to create it
160 if (vpIoTools::checkDirectory(logdirname) == false) {
161 try {
162 // Create the dirname
163 vpIoTools::makeDirectory(logdirname);
164 } catch (...) {
165 std::cerr << std::endl << "ERROR:" << std::endl;
166 std::cerr << " Cannot create " << logdirname << std::endl;
167 return EXIT_FAILURE;
168 }
169 }
170 std::string logfilename;
171 logfilename = logdirname + "/log.dat";
172
173 // Open the log file name
174 std::ofstream flog(logfilename.c_str());
175
176 try {
177 vpServo task;
178
180 int i;
181
185 g.open(I);
186
187#ifdef VISP_HAVE_X11
188 vpDisplayX display(I, 100, 100, "Current image");
189#elif defined(HAVE_OPENCV_HIGHGUI)
190 vpDisplayOpenCV display(I, 100, 100, "Current image");
191#elif defined(VISP_HAVE_GTK)
192 vpDisplayGTK display(I, 100, 100, "Current image");
193#endif
194
195 g.acquire(I);
196
199
200 std::cout << std::endl;
201 std::cout << "-------------------------------------------------------" << std::endl;
202 std::cout << " Test program for vpServo " << std::endl;
203 std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
204 std::cout << " Use of the Afma6 robot " << std::endl;
205 std::cout << " Interaction matrix computed with the current features " << std::endl;
206 std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl;
207 std::cout << "-------------------------------------------------------" << std::endl;
208 std::cout << std::endl;
209
210 vpDot2 dot[4];
211 vpImagePoint cog;
212
213 std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl;
214 for (i = 0; i < 4; i++) {
215 dot[i].initTracking(I);
216 cog = dot[i].getCog();
219 }
220
222 vpRobotAfma6 robot;
223
224 // Load the end-effector to camera frame transformation obtained
225 // using a camera intrinsic model with distortion
226 robot.init(vpAfma6::TOOL_CCMOP, projModel);
227
229 // Update camera parameters
230 robot.getCameraParameters(cam, I);
231
232 // Sets the current position of the visual feature
233 vpFeaturePoint p[4];
234 for (i = 0; i < 4; i++)
235 vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure
236
237 // Set the position of the square target in a frame which origin is
238 // centered in the middle of the square
239 vpPoint point[4];
240 point[0].setWorldCoordinates(-L, -L, 0);
241 point[1].setWorldCoordinates(L, -L, 0);
242 point[2].setWorldCoordinates(L, L, 0);
243 point[3].setWorldCoordinates(-L, L, 0);
244
245 // Initialise a desired pose to compute s*, the desired 2D point features
247 vpTranslationVector cto(0, 0, 0.7); // tz = 0.7 meter
249 vpMath::rad(0)); // No rotations
250 vpRotationMatrix cRo(cro); // Build the rotation matrix
251 cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
252
253 // Sets the desired position of the 2D visual feature
254 vpFeaturePoint pd[4];
255 // Compute the desired position of the features from the desired pose
256 for (int i = 0; i < 4; i++) {
257 vpColVector cP, p;
258 point[i].changeFrame(cMo, cP);
259 point[i].projection(cP, p);
260
261 pd[i].set_x(p[0]);
262 pd[i].set_y(p[1]);
263 pd[i].set_Z(cP[2]);
264 }
265
266 // Define the task
267 // - we want an eye-in-hand control law
268 // - robot is controlled in the camera frame
269 // - Interaction matrix is computed with the current visual features
272
273 // We want to see a point on a point
274 std::cout << std::endl;
275 for (i = 0; i < 4; i++)
276 task.addFeature(p[i], pd[i]);
277
278 // Set the proportional gain
279 task.setLambda(0.1);
280
281 // Display task information
282 task.print();
283
284 // Initialise the velocity control of the robot
286
287 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
288
289 bool init_pose_from_linear_method = true;
290
291 for (;;) {
292 // Acquire a new image from the camera
293 g.acquire(I);
294
295 // Display this image
297
298 // For each point...
299 for (i = 0; i < 4; i++) {
300 // Achieve the tracking of the dot in the image
301 dot[i].track(I);
302 // Get the dot cog
303 cog = dot[i].getCog();
304 // Display a green cross at the center of gravity position in the
305 // image
307 }
308
309 // At first iteration, we initialise non linear pose estimation with a linear approach.
310 // For the other iterations, non linear pose estimation is initialized with the pose estimated at previous iteration of the loop
311 compute_pose(point, dot, 4, cam, cMo, init_pose_from_linear_method);
312 if (init_pose_from_linear_method) {
313 init_pose_from_linear_method = false;
314 }
315
316 for (i = 0; i < 4; i++) {
317 // Update the point feature from the dot location
318 vpFeatureBuilder::create(p[i], cam, dot[i]);
319 // Set the feature Z coordinate from the pose
320 vpColVector cP;
321 point[i].changeFrame(cMo, cP);
322
323 p[i].set_Z(cP[2]);
324 }
325
326 // Printing on stdout concerning task information
327 // task.print() ;
328
329 vpColVector v;
330 // Compute the visual servoing skew vector
331 v = task.computeControlLaw();
332
333 // Display the current and desired feature points in the image display
334 vpServoDisplay::display(task, cam, I);
335
336 // Apply the computed camera velocities to the robot
338
339 // Save velocities applied to the robot in the log file
340 // v[0], v[1], v[2] correspond to camera translation velocities in m/s
341 // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
342 flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
343
344 // Get the measured joint velocities of the robot
345 vpColVector qvel;
347 // Save measured joint velocities of the robot in the log file:
348 // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
349 // velocities in m/s
350 // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
351 // velocities in rad/s
352 flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
353
354 // Get the measured joint positions of the robot
355 vpColVector q;
356 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
357 // Save measured joint positions of the robot in the log file
358 // - q[0], q[1], q[2] correspond to measured joint translation
359 // positions in m
360 // - q[3], q[4], q[5] correspond to measured joint rotation
361 // positions in rad
362 flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
363
364 // Save feature error (s-s*) for the 4 feature points. For each feature
365 // point, we have 2 errors (along x and y axis). This error is
366 // expressed in meters in the camera frame
367 flog << (task.getError()).t() << " "; // s-s* for points
368
369 // Save the current cMo pose: translations in meters, rotations (rx, ry,
370 // rz) in radians
371 flog << cto[0] << " " << cto[1] << " " << cto[2] << " " // translation
372 << cro[0] << " " << cro[1] << " " << cro[2] << std::endl; // rot
373
374 // Flush the display
376 }
377
378 flog.close(); // Close the log file
379
380 // Display task information
381 task.print();
382
383 return EXIT_SUCCESS;
384 } catch (const vpException &e) {
385 flog.close(); // Close the log file
386
387 std::cout << "Test failed with exception: " << e << std::endl;
388 return EXIT_FAILURE;
389 }
390}
391
392#else
393int main()
394{
395 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
396 return EXIT_SUCCESS;
397}
398
399#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
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
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 set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
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
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
@ VIRTUAL_VS
Definition vpPose.h:96
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
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
@ CURRENT
Definition vpServo.h:179
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.