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