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