Visual Servoing Platform version 3.5.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
71static 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
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
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
199int 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);
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
239int 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:1365
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
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()