Visual Servoing Platform  version 3.0.1
manServo4PointsDisplay.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Simulation of a visual servoing with display.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
49 #include <visp3/core/vpConfig.h>
50 #include <visp3/core/vpDebug.h>
51 
52 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
53 
54 #include <visp3/core/vpImage.h>
55 #include <visp3/core/vpCameraParameters.h>
56 #include <visp3/core/vpTime.h>
57 #include <visp3/core/vpImage.h>
58 #include <visp3/core/vpImageConvert.h>
59 #include <visp3/gui/vpDisplayX.h>
60 #include <visp3/gui/vpDisplayGTK.h>
61 #include <visp3/gui/vpDisplayGDI.h>
62 #include <visp3/gui/vpDisplayOpenCV.h>
63 
64 #include <visp3/core/vpMath.h>
65 #include <visp3/core/vpHomogeneousMatrix.h>
66 #include <visp3/vision/vpPose.h>
67 #include <visp3/visual_features/vpFeaturePoint.h>
68 #include <visp3/vs/vpServo.h>
69 #include <visp3/vs/vpServoDisplay.h>
70 #include <visp3/robot/vpSimulatorCamera.h>
71 #include <visp3/visual_features/vpFeatureBuilder.h>
72 #include <visp3/core/vpIoTools.h>
73 
74 int main()
75 {
76  try {
78  // sets the initial camera location
79  vpHomogeneousMatrix cMo(0.3,0.2,3,
81  vpHomogeneousMatrix wMo; // Set to identity
82  vpHomogeneousMatrix wMc; // Camera position in the world frame
83 
85  // initialize the robot
86  vpSimulatorCamera robot ;
87  robot.setSamplingTime(0.04); // 40ms
88  wMc = wMo * cMo.inverse();
89  robot.setPosition(wMc) ;
90 
91  //initialize the camera parameters
92  vpCameraParameters cam(800,800,240,180);
93 
94  //Image definition
95  unsigned int height = 360 ;
96  unsigned int width = 480 ;
97  vpImage<unsigned char> I(height,width);
98 
99  //Display initialization
100 #if defined(VISP_HAVE_X11)
101  vpDisplayX disp;
102 #elif defined(VISP_HAVE_GTK)
103  vpDisplayGTK disp;
104 #elif defined(VISP_HAVE_GDI)
105  vpDisplayGDI disp;
106 #elif defined(VISP_HAVE_OPENCV)
107  vpDisplayOpenCV disp;
108 #endif
109  disp.init(I,100,100,"Simulation display");
110 
112  // Desired visual features initialization
113 
114  // sets the points coordinates in the object frame (in meter)
115  vpPoint point[4] ;
116  point[0].setWorldCoordinates(-0.1,-0.1,0) ;
117  point[1].setWorldCoordinates(0.1,-0.1,0) ;
118  point[2].setWorldCoordinates(0.1,0.1,0) ;
119  point[3].setWorldCoordinates(-0.1,0.1,0) ;
120 
121  // sets the desired camera location
122  vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
123 
124  // computes the 3D point coordinates in the camera frame and its 2D coordinates
125  for (int i = 0 ; i < 4 ; i++)
126  point[i].project(cMo_d) ;
127 
128  // creates the associated features
129  vpFeaturePoint pd[4] ;
130  for (int i = 0 ; i < 4 ; i++)
131  vpFeatureBuilder::create(pd[i],point[i]) ;
132 
133 
135  // Current visual features initialization
136 
137  // computes the 3D point coordinates in the camera frame and its 2D coordinates
138  for (int i = 0 ; i < 4 ; i++)
139  point[i].project(cMo) ;
140 
141  // creates the associated features
142  vpFeaturePoint p[4] ;
143  for (int i = 0 ; i < 4 ; i++)
144  vpFeatureBuilder::create(p[i],point[i]) ;
145 
146 
148  // Task defintion
149  vpServo task ;
150  // we want an eye-in-hand control law ;
153 
154  // Set the position of the camera in the end-effector frame
155  vpHomogeneousMatrix cMe ;
156  vpVelocityTwistMatrix cVe(cMe) ;
157  task.set_cVe(cVe) ;
158  // Set the Jacobian (expressed in the end-effector frame)
159  vpMatrix eJe ;
160  robot.get_eJe(eJe) ;
161  task.set_eJe(eJe) ;
162 
163  // we want to see a point on a point
164  for (int i = 0 ; i < 4 ; i++)
165  task.addFeature(p[i],pd[i]) ;
166  // Set the gain
167  task.setLambda(1.0) ;
168  // Print the current information about the task
169  task.print();
170 
171 
173  // The control loop
174  int k = 0;
175  while(k++ < 200){
176  double t = vpTime::measureTimeMs();
177 
178  // Display the image background
180 
181  // Update the current features
182  for (int i = 0 ; i < 4 ; i++)
183  {
184  point[i].project(cMo) ;
185  vpFeatureBuilder::create(p[i],point[i]) ;
186  }
187 
188  // Display the task features (current and desired)
189  vpServoDisplay::display(task,cam,I);
190  vpDisplay::flush(I);
191 
192  // Update the robot Jacobian
193  robot.get_eJe(eJe) ;
194  task.set_eJe(eJe) ;
195 
196  // Compute the control law
197  vpColVector v = task.computeControlLaw() ;
198 
199  // Send the computed velocity to the robot and compute the new robot position
201  wMc = robot.getPosition();
202  cMo = wMc.inverse() * wMo;
203 
204  // Print the current information about the task
205  task.print();
206 
207  // Wait 40 ms
208  vpTime::wait(t,40);
209  }
210  task.kill();
211  return 0;
212  }
213  catch(vpException &e) {
214  std::cout << "Catch an exception: " << e << std::endl;
215  return 1;
216  }
217 }
218 
219 #else
220 int
221 main()
222 { vpTRACE("You should install GTK") ;
223 
224 }
225 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:157
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:460
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:512
error that can be emited by ViSP classes.
Definition: vpException.h:73
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
vpHomogeneousMatrix inverse() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
vpHomogeneousMatrix getPosition() const
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
Class that defines what is a point.
Definition: vpPoint.h:59
virtual void setSamplingTime(const double &delta_t)
void kill()
Definition: vpServo.cpp:191
vpColVector computeControlLaw()
Definition: vpServo.cpp:954
#define vpTRACE
Definition: vpDebug.h:414
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:391
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
Implementation of a velocity twist matrix and operations on such kind of matrices.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:585
static double rad(double deg)
Definition: vpMath.h:104
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:111
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:435
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:314
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222
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)