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