ViSP  3.0.0
manSimu4Points.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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 visualization.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
51 #include <visp3/core/vpConfig.h>
52 #include <visp3/core/vpDebug.h>
53 
54 
55 #if (defined(VISP_HAVE_COIN3D_AND_GUI))
56 
57 #include <visp3/core/vpImage.h>
58 #include <visp3/core/vpCameraParameters.h>
59 #include <visp3/core/vpTime.h>
60 #include <visp3/ar/vpSimulator.h>
61 
62 
63 #include <visp3/core/vpMath.h>
64 #include <visp3/core/vpHomogeneousMatrix.h>
65 #include <visp3/visual_features/vpFeaturePoint.h>
66 #include <visp3/vs/vpServo.h>
67 #include <visp3/robot/vpSimulatorCamera.h>
68 #include <visp3/visual_features/vpFeatureBuilder.h>
69 #include <visp3/io/vpParseArgv.h>
70 #include <visp3/core/vpIoTools.h>
71 
72 static
73 void *mainLoop (void *_simu)
74 {
75  // pointer copy of the vpSimulator instance
76  vpSimulator *simu = (vpSimulator *)_simu ;
77 
78  // Simulation initialization
79  simu->initMainApplication() ;
80 
82  // sets the initial camera location
83  vpHomogeneousMatrix cMo(-0.3,-0.2,3,
85  vpHomogeneousMatrix wMo; // Set to identity
86  vpHomogeneousMatrix wMc; // Camera position in the world frame
87 
89  // Initialize the robot
90  vpSimulatorCamera robot ;
91  robot.setSamplingTime(0.04); // 40ms
92  wMc = wMo * cMo.inverse();
93  robot.setPosition(wMc) ;
94  // Send the robot position to the visualizator
95  simu->setCameraPosition(cMo) ;
96  // Initialize the camera parameters
97  vpCameraParameters cam ;
98  simu->getCameraParameters(cam);
99 
101  // Desired visual features initialization
102 
103  // sets the points coordinates in the object frame (in meter)
104  vpPoint point[4] ;
105  point[0].setWorldCoordinates(-0.1,-0.1,0) ;
106  point[1].setWorldCoordinates(0.1,-0.1,0) ;
107  point[2].setWorldCoordinates(0.1,0.1,0) ;
108  point[3].setWorldCoordinates(-0.1,0.1,0) ;
109 
110  // sets the desired camera location
111  vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
112 
113  // computes the 3D point coordinates in the camera frame and its 2D coordinates
114  for (int i = 0 ; i < 4 ; i++)
115  point[i].project(cMo_d) ;
116 
117  // creates the associated features
118  vpFeaturePoint pd[4] ;
119  for (int i = 0 ; i < 4 ; i++)
120  vpFeatureBuilder::create(pd[i],point[i]) ;
121 
123  // Current visual features initialization
124 
125  // computes the 3D point coordinates in the camera frame and its 2D coordinates
126  for (int i = 0 ; i < 4 ; i++)
127  point[i].project(cMo) ;
128 
129  // creates the associated features
130  vpFeaturePoint p[4] ;
131  for (int i = 0 ; i < 4 ; i++)
132  vpFeatureBuilder::create(p[i],point[i]) ;
133 
134 
136  // Task defintion
137  vpServo task ;
138  // we want an eye-in-hand control law ;
141 
142  // Set the position of the camera in the end-effector frame
143  vpHomogeneousMatrix cMe ;
144  vpVelocityTwistMatrix cVe(cMe) ;
145  task.set_cVe(cVe) ;
146  // Set the Jacobian (expressed in the end-effector frame)
147  vpMatrix eJe ;
148  robot.get_eJe(eJe) ;
149  task.set_eJe(eJe) ;
150 
151  // we want to see a point on a point
152  for (int i = 0 ; i < 4 ; i++)
153  task.addFeature(p[i],pd[i]) ;
154  // Set the gain
155  task.setLambda(1.0) ;
156  // Print the current information about the task
157  task.print();
158 
159  vpTime::wait(500);
161  // The control loop
162  int k = 0;
163  while(k++ < 200){
164  double t = vpTime::measureTimeMs();
165 
166 
167  // Update the current features
168  for (int i = 0 ; i < 4 ; i++)
169  {
170  point[i].project(cMo) ;
171  vpFeatureBuilder::create(p[i],point[i]) ;
172  }
173 
174  // Update the robot Jacobian
175  robot.get_eJe(eJe) ;
176  task.set_eJe(eJe) ;
177 
178  // Compute the control law
179  vpColVector v = task.computeControlLaw() ;
180 
181  // Send the computed velocity to the robot and compute the new robot position
183  wMc = robot.getPosition();
184  cMo = wMc.inverse() * wMo;
185 
186  // Send the robot position to the visualizator
187  simu->setCameraPosition(cMo) ;
188 
189  // Print the current information about the task
190  task.print();
191 
192  // Wait 40 ms
193  vpTime::wait(t,40);
194  }
195  task.kill();
196  simu->closeMainApplication() ;
197 
198 
199  void *a=NULL ;
200  return a ;
201  // return (void *);
202 }
203 
204 
205 int
206 main()
207 {
208  try {
209  vpSimulator simu ;
210 
211  // Internal view initialization : view from the robot camera
212  simu.initInternalViewer(480, 360) ;
213  // External view initialization : view from an external camera
214  simu.initExternalViewer(300, 300) ;
215 
216  // Inernal camera paramters initialization
217  vpCameraParameters cam(800,800,240,180) ;
218  simu.setInternalCameraParameters(cam) ;
219 
220  vpTime::wait(1000) ;
221  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
222  std::string ipath = vpIoTools::getViSPImagesDataPath();
223  std::string filename = "./4points.iv";
224 
225  // Set the default input path
226  if (! ipath.empty())
227  filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
228 
229  std::cout << "Load : " << filename << std::endl
230  << "This file should be in the working directory" << std::endl;
231 
232  simu.load(filename.c_str());
233 
234  // Run the main loop
235  simu.initApplication(&mainLoop) ;
236  // Run the simulator
237  simu.mainLoop() ;
238  return 0;
239  }
240  catch(vpException e) {
241  std::cout << "Catch an exception: " << e << std::endl;
242  return 1;
243  }
244 }
245 
246 #else
247 int
248 main()
249 {
250  vpTRACE("You should install Coin3D and/or GTK") ;
251 }
252 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1091
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:98
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:459
void closeMainApplication()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:446
error that can be emited by ViSP classes.
Definition: vpException.h:73
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:297
virtual void mainLoop()
activate the mainloop
Class that defines what is a point.
Definition: vpPoint.h:59
virtual void setSamplingTime(const double &delta_t)
void kill()
Definition: vpServo.cpp:186
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
Definition: vpServo.cpp:899
#define vpTRACE
Definition: vpDebug.h:414
static std::string createFilePath(const std::string &parent, const std::string child)
Definition: vpIoTools.cpp:1265
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:390
void load(const char *file_name)
load an iv file
vpHomogeneousMatrix getPosition() const
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:519
static double rad(double deg)
Definition: vpMath.h:104
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
void initMainApplication()
perform some initialization in the main program thread
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:434
vpHomogeneousMatrix inverse() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:248
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:217