Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6Segment2DCamVelocity.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 * Authors:
37 * Filip Novotny
38 *
39*****************************************************************************/
40
50#include <stdlib.h>
51#include <vector>
52#include <visp3/core/vpConfig.h>
53#include <visp3/core/vpDebug.h> // Debug trace
54#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
55
56#include <visp3/blob/vpDot.h>
57#include <visp3/core/vpDisplay.h>
58#include <visp3/core/vpException.h>
59#include <visp3/core/vpHomogeneousMatrix.h>
60#include <visp3/core/vpImage.h>
61#include <visp3/core/vpImagePoint.h>
62#include <visp3/core/vpIoTools.h>
63#include <visp3/core/vpMath.h>
64#include <visp3/core/vpPoint.h>
65#include <visp3/gui/vpDisplayGTK.h>
66#include <visp3/gui/vpDisplayOpenCV.h>
67#include <visp3/gui/vpDisplayX.h>
68#include <visp3/robot/vpRobotAfma6.h>
69#include <visp3/sensor/vp1394TwoGrabber.h>
70#include <visp3/visual_features/vpFeatureBuilder.h>
71#include <visp3/visual_features/vpFeatureSegment.h>
72#include <visp3/vs/vpServo.h>
73#include <visp3/vs/vpServoDisplay.h>
74
75int main()
76{
77 // Log file creation in /tmp/$USERNAME/log.dat
78 // This file contains by line:
79 // - the 6 computed cam velocities (m/s, rad/s) to achieve the task
80 // - the 6 mesured joint velocities (m/s, rad/s)
81 // - the 6 mesured joint positions (m, rad)
82 // - the 2 values of s - s*
83 std::string username;
84 // Get the user login name
85 vpIoTools::getUserName(username);
86
87 // Create a log filename to save velocities...
88 std::string logdirname;
89 logdirname = "/tmp/" + username;
90
91 // Test if the output path exist. If no try to create it
92 if (vpIoTools::checkDirectory(logdirname) == false) {
93 try {
94 // Create the dirname
95 vpIoTools::makeDirectory(logdirname);
96 } catch (...) {
97 std::cerr << std::endl << "ERROR:" << std::endl;
98 std::cerr << " Cannot create " << logdirname << std::endl;
99 return EXIT_FAILURE;
100 }
101 }
102 std::string logfilename;
103 logfilename = logdirname + "/log.dat";
104
105 // Open the log file name
106 std::ofstream flog(logfilename.c_str());
107
108 try {
109 vpServo task;
110
112
116 g.open(I);
117
118 g.acquire(I);
119
120#ifdef VISP_HAVE_X11
121 vpDisplayX display(I, 100, 100, "Current image");
122#elif defined(HAVE_OPENCV_HIGHGUI)
123 vpDisplayOpenCV display(I, 100, 100, "Current image");
124#elif defined(VISP_HAVE_GTK)
125 vpDisplayGTK display(I, 100, 100, "Current image");
126#endif
127
130
131 std::vector<vpDot> dot_d(2), dot(2);
132 vpFeatureSegment seg_d, seg;
133 vpImagePoint cog;
134 vpRobotAfma6 robot;
136
137 // Update camera parameters
138 robot.getCameraParameters(cam, I);
139 std::cout << "define the initial segment" << std::endl;
140
141 for (std::vector<vpDot>::iterator i = dot.begin(); i != dot.end(); ++i) {
142 std::cout << "Click on a dot..." << std::endl;
143 i->initTracking(I);
144 cog = i->getCog();
147 }
148 vpFeatureBuilder::create(seg, cam, dot[0], dot[1]);
149 seg.display(cam, I, vpColor::red);
151 std::cout << "define the destination segment" << std::endl;
152 for (std::vector<vpDot>::iterator i = dot_d.begin(); i != dot_d.end(); ++i) {
153 vpImagePoint ip;
154 vpDisplay::getClick(I, ip);
155 *i = vpDot(ip);
157 }
158 vpFeatureBuilder::create(seg_d, cam, dot_d[0], dot_d[1]);
159 seg_d.setZ1(1.);
160 seg_d.setZ2(1.);
161
162 seg_d.display(cam, I);
164 // define the task
165 // - we want an eye-in-hand control law
166 // - robot is controlled in the camera frame
169
170 // - we want to see both segments
171 task.addFeature(seg, seg_d);
172
173 // - set the constant gain
174 task.setLambda(0.8);
175
176 // Display task information
177 task.print();
178
179 // Now the robot will be controlled in velocity
181
182 std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
183 for (;;) {
184 // Acquire a new image from the camera
185 g.acquire(I);
186
187 // Display this image
189
190 // Achieve the tracking of the dot in the image
191 for (std::vector<vpDot>::iterator i = dot.begin(); i != dot.end(); ++i) {
192 i->track(I);
193 }
194
195 // Update the segment feature from the dot locations
196 vpFeatureBuilder::create(seg, cam, dot[0], dot[1]);
197
198 vpColVector v;
199 // Compute the visual servoing skew vector
200 v = task.computeControlLaw();
201
202 // Display the current and desired feature segments in the image display
203 vpServoDisplay::display(task, cam, I);
204
205 // Apply the computed joint velocities to the robot
207
208 // Save feature error (s-s*) for the feature segment. For this feature
209 // segments, we have 4 errors (Xc,Yc,l,alpha).
210 flog << (task.getError()).t() << std::endl;
211
212 // Flush the display
214 }
215
216 flog.close(); // Close the log file
217
218 // Display task information
219 task.print();
220
221 return EXIT_SUCCESS;
222 } catch (const vpException &e) {
223 flog.close(); // Close the log file
224 std::cout << "Test failed with exception: " << e << std::endl;
225 return EXIT_FAILURE;
226 }
227}
228
229#else
230int main()
231{
232 std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
233 return EXIT_SUCCESS;
234}
235#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 red
Definition vpColor.h:211
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 bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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
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 segment visual features. This class allow to consider two sets of visual feat...
void setZ2(double val)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void setZ1(double val)
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 gantry robot named Afma6.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ 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)
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
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