Visual Servoing Platform version 3.5.0
vpRobotWireFrameSimulator.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 * Basic class used to make robot simulators.
33 *
34 * Authors:
35 * Nicolas Melchior
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpConfig.h>
40
41#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
42#include <visp3/robot/vpRobotWireFrameSimulator.h>
43#include <visp3/robot/vpSimulatorViper850.h>
44
45#include "../wireframe-simulator/vpBound.h"
46#include "../wireframe-simulator/vpScene.h"
47#include "../wireframe-simulator/vpVwstack.h"
48
53 : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
54 artCoord(), artVel(), velocity(),
55#if defined(_WIN32)
56#elif defined(VISP_HAVE_PTHREAD)
57 thread(), attr(),
58#endif
59 mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(), displayBusy(false),
60 robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true), cameraParam(),
61#if defined(VISP_HAVE_DISPLAY)
62 display(),
63#endif
64 displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false), setVelocityCalled(false),
65 verbose_(false)
66{
67 setSamplingTime(0.010);
69 I.resize(480, 640);
70 I = 255;
71#if defined(VISP_HAVE_DISPLAY)
72 display.init(I, 0, 0, "The External view");
73#endif
74
75 // pid_t pid = getpid();
76 // setpriority (PRIO_PROCESS, pid, 19);
77}
78
84 : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
85 artCoord(), artVel(), velocity(),
86#if defined(_WIN32)
87#elif defined(VISP_HAVE_PTHREAD)
88 thread(), attr(),
89#endif
90 /* thread(), attr(), */ mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
91 displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
92 cameraParam(),
93#if defined(VISP_HAVE_DISPLAY)
94 display(),
95#endif
96 displayType(MODEL_3D), displayAllowed(do_display), constantSamplingTimeMode(false), setVelocityCalled(false),
97 verbose_(false)
98{
99 setSamplingTime(0.010);
100 velocity.resize(6);
101 I.resize(480, 640);
102 I = 255;
103
104#if defined(VISP_HAVE_DISPLAY)
105 if (do_display)
106 this->display.init(I, 0, 0, "The External view");
107#endif
108
109 // pid_t pid = getpid();
110 // setpriority (PRIO_PROCESS, pid, 19);
111}
112
117
133{
134 if (displayCamera) {
135 free_Bound_scene(&(this->camera));
136 }
137 vpWireFrameSimulator::initScene(obj, desired_object);
138 if (displayCamera) {
139 free_Bound_scene(&(this->camera));
140 }
141 displayCamera = false;
142}
143
155void vpRobotWireFrameSimulator::initScene(const char *obj, const char *desired_object)
156{
157 if (displayCamera) {
158 free_Bound_scene(&(this->camera));
159 }
160 vpWireFrameSimulator::initScene(obj, desired_object);
161 if (displayCamera) {
162 free_Bound_scene(&(this->camera));
163 }
164 displayCamera = false;
165}
166
180{
181 if (displayCamera) {
182 free_Bound_scene(&(this->camera));
183 }
185 if (displayCamera) {
186 free_Bound_scene(&(this->camera));
187 }
188 displayCamera = false;
189}
190
202{
203 if (displayCamera) {
204 free_Bound_scene(&(this->camera));
205 }
207 if (displayCamera) {
208 free_Bound_scene(&(this->camera));
209 }
210 displayCamera = false;
211}
212
225{
226
227 if (!sceneInitialized)
228 throw;
229
230 double u;
231 double v;
232 // if(px_int != 1 && py_int != 1)
233 // we assume px_int and py_int > 0
234 if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
235 (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
236 u = (double)I_.getWidth() / (2 * px_int);
237 v = (double)I_.getHeight() / (2 * py_int);
238 } else {
239 u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
240 v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
241 }
242
243 float o44c[4][4], o44cd[4][4], x, y, z;
244 Matrix id = IDENTITY_MATRIX;
245
247 get_fMi(fMit);
248 this->cMo = fMit[size_fMi - 1].inverse() * fMo;
249 this->cMo = rotz * cMo;
250
251 vp2jlc_matrix(cMo.inverse(), o44c);
252 vp2jlc_matrix(cdMo.inverse(), o44cd);
253
254 while (get_displayBusy())
255 vpTime::wait(2);
256
257 add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
258 x = o44c[2][0] + o44c[3][0];
259 y = o44c[2][1] + o44c[3][1];
260 z = o44c[2][2] + o44c[3][2];
261 add_vwstack("start", "vrp", x, y, z);
262 add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
263 add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
264 add_vwstack("start", "window", -u, u, -v, v);
265 if (displayObject)
266 display_scene(id, this->scene, I_, curColor);
267
268 add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
269 x = o44cd[2][0] + o44cd[3][0];
270 y = o44cd[2][1] + o44cd[3][1];
271 z = o44cd[2][2] + o44cd[3][2];
272 add_vwstack("start", "vrp", x, y, z);
273 add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
274 add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
275 add_vwstack("start", "window", -u, u, -v, v);
277 if (desiredObject == D_TOOL)
279 else
281 }
282 delete[] fMit;
283 set_displayBusy(false);
284}
285
298{
299
300 if (!sceneInitialized)
301 throw;
302
303 double u;
304 double v;
305 // if(px_int != 1 && py_int != 1)
306 // we assume px_int and py_int > 0
307 if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
308 (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
309 u = (double)I.getWidth() / (2 * px_int);
310 v = (double)I.getHeight() / (2 * py_int);
311 } else {
312 u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
313 v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
314 }
315
316 float o44c[4][4], o44cd[4][4], x, y, z;
317 Matrix id = IDENTITY_MATRIX;
318
320 get_fMi(fMit);
321 this->cMo = fMit[size_fMi - 1].inverse() * fMo;
322 this->cMo = rotz * cMo;
323
324 vp2jlc_matrix(cMo.inverse(), o44c);
325 vp2jlc_matrix(cdMo.inverse(), o44cd);
326
327 while (get_displayBusy())
328 vpTime::wait(2);
329
330 add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
331 x = o44c[2][0] + o44c[3][0];
332 y = o44c[2][1] + o44c[3][1];
333 z = o44c[2][2] + o44c[3][2];
334 add_vwstack("start", "vrp", x, y, z);
335 add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
336 add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
337 add_vwstack("start", "window", -u, u, -v, v);
338 if (displayObject) {
339 display_scene(id, this->scene, I_, curColor);
340 }
341
342 add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
343 x = o44cd[2][0] + o44cd[3][0];
344 y = o44cd[2][1] + o44cd[3][1];
345 z = o44cd[2][2] + o44cd[3][2];
346 add_vwstack("start", "vrp", x, y, z);
347 add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
348 add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
349 add_vwstack("start", "window", -u, u, -v, v);
351 if (desiredObject == D_TOOL)
353 else
355 }
356 delete[] fMit;
357 set_displayBusy(false);
358}
359
366{
367 vpHomogeneousMatrix cMoTemp;
369 get_fMi(fMit);
370 cMoTemp = fMit[size_fMi - 1].inverse() * fMo;
371 delete[] fMit;
372 return cMoTemp;
373}
374
375#elif !defined(VISP_BUILD_SHARED_LIBS)
376// Work arround to avoid warning:
377// libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
378void dummy_vpRobotWireFrameSimulator(){};
379#endif
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static const vpColor red
Definition: vpColor.h:217
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
unsigned int getWidth() const
Definition: vpImage.h:246
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
unsigned int getHeight() const
Definition: vpImage.h:188
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
This class aims to be a basis used to create all the robot simulators.
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
void set_displayBusy(const bool &status)
void getInternalView(vpImage< vpRGBa > &I)
virtual void get_fMi(vpHomogeneousMatrix *fMit)=0
void setSamplingTime(const double &delta_t)
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpSceneDesiredObject desiredObject
@ D_TOOL
A cylindrical tool is attached to the camera.
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix fMo
vpHomogeneousMatrix cMo
vpHomogeneousMatrix rotz
vpHomogeneousMatrix cdMo
VISP_EXPORT int wait(double t0, double t)