ViSP
manSimu4Points.cpp
1 /****************************************************************************
2  *
3  * $Id: manSimu4Points.cpp 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Simulation of a visual servoing with visualization.
35  *
36  * Authors:
37  * Eric Marchand
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
54 #include <visp/vpConfig.h>
55 #include <visp/vpDebug.h>
56 
57 
58 #if (defined(VISP_HAVE_COIN_AND_GUI))
59 
60 #include <visp/vpImage.h>
61 #include <visp/vpCameraParameters.h>
62 #include <visp/vpTime.h>
63 #include <visp/vpSimulator.h>
64 
65 
66 #include <visp/vpMath.h>
67 #include <visp/vpHomogeneousMatrix.h>
68 #include <visp/vpFeaturePoint.h>
69 #include <visp/vpServo.h>
70 #include <visp/vpRobotCamera.h>
71 #include <visp/vpFeatureBuilder.h>
72 #include <visp/vpParseArgv.h>
73 #include <visp/vpIoTools.h>
74 
75 static
76 void *mainLoop (void *_simu)
77 {
78  // pointer copy of the vpSimulator instance
79  vpSimulator *simu = (vpSimulator *)_simu ;
80 
81  // Simulation initialization
82  simu->initMainApplication() ;
83 
84 
86  // sets the initial camera location
87  vpHomogeneousMatrix cMo(-0.3,-0.2,3,
89 
91  // Initialize the robot
92  vpRobotCamera robot ;
93  robot.setSamplingTime(0.04); // 40ms
94  robot.setPosition(cMo) ;
95  // Send the robot position to the visualizator
96  simu->setCameraPosition(cMo) ;
97  // Initialize the camera parameters
98  vpCameraParameters cam ;
99  simu->getCameraParameters(cam);
100 
102  // Desired visual features initialization
103 
104  // sets the points coordinates in the object frame (in meter)
105  vpPoint point[4] ;
106  point[0].setWorldCoordinates(-0.1,-0.1,0) ;
107  point[1].setWorldCoordinates(0.1,-0.1,0) ;
108  point[2].setWorldCoordinates(0.1,0.1,0) ;
109  point[3].setWorldCoordinates(-0.1,0.1,0) ;
110 
111  // sets the desired camera location
112  vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
113 
114  // computes the 3D point coordinates in the camera frame and its 2D coordinates
115  for (int i = 0 ; i < 4 ; i++)
116  point[i].project(cMo_d) ;
117 
118  // creates the associated features
119  vpFeaturePoint pd[4] ;
120  for (int i = 0 ; i < 4 ; i++)
121  vpFeatureBuilder::create(pd[i],point[i]) ;
122 
123 
124 
126  // Current visual features initialization
127 
128  // computes the 3D point coordinates in the camera frame and its 2D coordinates
129  for (int i = 0 ; i < 4 ; i++)
130  point[i].project(cMo) ;
131 
132  // creates the associated features
133  vpFeaturePoint p[4] ;
134  for (int i = 0 ; i < 4 ; i++)
135  vpFeatureBuilder::create(p[i],point[i]) ;
136 
137 
139  // Task defintion
140  vpServo task ;
141  // we want an eye-in-hand control law ;
144 
145  // Set the position of the camera in the end-effector frame
146  vpHomogeneousMatrix cMe ;
147  vpVelocityTwistMatrix cVe(cMe) ;
148  task.set_cVe(cVe) ;
149  // Set the Jacobian (expressed in the end-effector frame)
150  vpMatrix eJe ;
151  robot.get_eJe(eJe) ;
152  task.set_eJe(eJe) ;
153 
154  // we want to see a point on a point
155  for (int i = 0 ; i < 4 ; i++)
156  task.addFeature(p[i],pd[i]) ;
157  // Set the gain
158  task.setLambda(1.0) ;
159  // Print the current information about the task
160  task.print();
161 
162  vpTime::wait(500);
164  // The control loop
165  int k = 0;
166  while(k++ < 200){
167  double t = vpTime::measureTimeMs();
168 
169 
170  // Update the current features
171  for (int i = 0 ; i < 4 ; i++)
172  {
173  point[i].project(cMo) ;
174  vpFeatureBuilder::create(p[i],point[i]) ;
175  }
176 
177  // Update the robot Jacobian
178  robot.get_eJe(eJe) ;
179  task.set_eJe(eJe) ;
180 
181  // Compute the control law
182  vpColVector v = task.computeControlLaw() ;
183 
184  // Send the computed velocity to the robot and compute the new robot position
186  robot.getPosition(cMo) ;
187 
188  // Send the robot position to the visualizator
189  simu->setCameraPosition(cMo) ;
190 
191  // Print the current information about the task
192  task.print();
193 
194  // Wait 40 ms
195  vpTime::wait(t,40);
196  }
197  task.kill();
198  simu->closeMainApplication() ;
199 
200 
201  void *a=NULL ;
202  return a ;
203  // return (void *);
204 }
205 
206 
207 int
208 main()
209 {
210  try {
211  vpSimulator simu ;
212 
213  // Internal view initialization : view from the robot camera
214  simu.initInternalViewer(480, 360) ;
215  // External view initialization : view from an external camera
216  simu.initExternalViewer(300, 300) ;
217 
218  // Inernal camera paramters initialization
219  vpCameraParameters cam(800,800,240,180) ;
220  simu.setInternalCameraParameters(cam) ;
221 
222  vpTime::wait(1000) ;
223  // Load the scene
224  std::cout << "Load : ./4Points.iv" << std::endl
225  << "This file should be in the working directory" << std::endl;
226  simu.load("./4points.iv") ;
227 
228  // Run the main loop
229  simu.initApplication(&mainLoop) ;
230  // Run the simulator
231  simu.mainLoop() ;
232  return 0;
233  }
234  catch(vpException e) {
235  std::cout << "Catch an exception: " << e << std::endl;
236  return 1;
237  }
238 }
239 
240 #else
241 int
242 main()
243 {
244  vpTRACE("You should install Coin3D and/or GTK") ;
245 }
246 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpTRACE
Definition: vpDebug.h:418
void setPosition(const vpHomogeneousMatrix &cMw)
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:102
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:439
void closeMainApplication()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:449
error that can be emited by ViSP classes.
Definition: vpException.h:76
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void getCameraParameters(vpCameraParameters &cam)
get the intrinsic parameters of the camera
Definition: vpSimulator.h:299
static double measureTimeMs()
Definition: vpTime.cpp:86
virtual void mainLoop()
activate the mainloop
static int wait(double t0, double t)
Definition: vpTime.cpp:149
Class that defines what is a point.
Definition: vpPoint.h:65
virtual void setSamplingTime(const double &delta_t)
void kill()
Definition: vpServo.cpp:189
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
Definition: vpServo.cpp:902
Class that defines the simplest robot: a free flying camera.
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:370
void load(const char *file_name)
load an iv file
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:522
static double rad(double deg)
Definition: vpMath.h:100
void getPosition(vpHomogeneousMatrix &cMw) const
void initMainApplication()
perform some initialization in the main program thread
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void get_eJe(vpMatrix &eJe)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:414
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:251
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:220
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74