Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotWireFrameSimulator.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 * Basic class used to make robot simulators.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
39#include <visp3/robot/vpRobotWireFrameSimulator.h>
40#include <visp3/robot/vpSimulatorViper850.h>
41
42#include "../wireframe-simulator/vpBound.h"
43#include "../wireframe-simulator/vpScene.h"
44#include "../wireframe-simulator/vpVwstack.h"
45
50 : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
51 artCoord(), artVel(), velocity(),
52#if defined(_WIN32)
53#elif defined(VISP_HAVE_PTHREAD)
54 thread(), attr(),
55#endif
56 m_mutex_fMi(), m_mutex_eMc(), m_mutex_artVel(), m_mutex_artCoord(), m_mutex_velocity(), m_mutex_display(), m_mutex_robotStop(), m_mutex_frame(), m_mutex_setVelocityCalled(), m_mutex_scene(),
57 displayBusy(false),
58 robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true), cameraParam(),
59#if defined(VISP_HAVE_DISPLAY)
60 display(),
61#endif
62 displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false), setVelocityCalled(false),
63 verbose_(false)
64{
65 setSamplingTime(0.010);
67 I.resize(480, 640);
68 I = 255;
69#if defined(VISP_HAVE_DISPLAY)
70 display.init(I, 0, 0, "The External view");
71#endif
72}
73
79 : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
80 artCoord(), artVel(), velocity(),
81#if defined(_WIN32)
82#elif defined(VISP_HAVE_PTHREAD)
83 thread(), attr(),
84#endif
85 m_mutex_fMi(), m_mutex_eMc(), m_mutex_artVel(), m_mutex_artCoord(), m_mutex_velocity(), m_mutex_display(), m_mutex_robotStop(), m_mutex_frame(), m_mutex_setVelocityCalled(), m_mutex_scene(),
86 displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
87 cameraParam(),
88#if defined(VISP_HAVE_DISPLAY)
89 display(),
90#endif
91 displayType(MODEL_3D), displayAllowed(do_display), constantSamplingTimeMode(false), setVelocityCalled(false),
92 verbose_(false)
93{
94 setSamplingTime(0.010);
96 I.resize(480, 640);
97 I = 255;
98
99#if defined(VISP_HAVE_DISPLAY)
100 if (do_display)
101 this->display.init(I, 0, 0, "The External view");
102#endif
103}
104
111
127{
129 if (displayCamera) {
130 free_Bound_scene(&(this->camera));
131 }
132 vpWireFrameSimulator::initScene(obj, desired_object);
133 if (displayCamera) {
134 free_Bound_scene(&(this->camera));
135 }
136 displayCamera = false;
138}
139
151void vpRobotWireFrameSimulator::initScene(const char *obj, const char *desired_object)
152{
153 if (displayCamera) {
154 free_Bound_scene(&(this->camera));
155 }
156 vpWireFrameSimulator::initScene(obj, desired_object);
157 if (displayCamera) {
158 free_Bound_scene(&(this->camera));
159 }
160 displayCamera = false;
161}
162
176{
177 if (displayCamera) {
178 free_Bound_scene(&(this->camera));
179 }
181 if (displayCamera) {
182 free_Bound_scene(&(this->camera));
183 }
184 displayCamera = false;
185}
186
198{
199 if (displayCamera) {
200 free_Bound_scene(&(this->camera));
201 }
203 if (displayCamera) {
204 free_Bound_scene(&(this->camera));
205 }
206 displayCamera = false;
207}
208
221{
222
223 if (!sceneInitialized)
224 throw;
225
226 double u;
227 double v;
228 // if(px_int != 1 && py_int != 1)
229 // we assume px_int and py_int > 0
230 if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
231 (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
232 u = (double)I_.getWidth() / (2 * px_int);
233 v = (double)I_.getHeight() / (2 * py_int);
234 } else {
235 u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
236 v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
237 }
238
239 float o44c[4][4], o44cd[4][4], x, y, z;
240 Matrix id = IDENTITY_MATRIX;
241
243 get_fMi(fMit);
244 this->cMo = fMit[size_fMi - 1].inverse() * fMo;
245 this->cMo = rotz * cMo;
246
247 vp2jlc_matrix(cMo.inverse(), o44c);
248 vp2jlc_matrix(cdMo.inverse(), o44cd);
249
250 while (get_displayBusy())
251 vpTime::wait(2);
252
253 add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
254 x = o44c[2][0] + o44c[3][0];
255 y = o44c[2][1] + o44c[3][1];
256 z = o44c[2][2] + o44c[3][2];
257 add_vwstack("start", "vrp", x, y, z);
258 add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
259 add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
260 add_vwstack("start", "window", -u, u, -v, v);
261 if (displayObject)
262 display_scene(id, this->scene, I_, curColor);
263
264 add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
265 x = o44cd[2][0] + o44cd[3][0];
266 y = o44cd[2][1] + o44cd[3][1];
267 z = o44cd[2][2] + o44cd[3][2];
268 add_vwstack("start", "vrp", x, y, z);
269 add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
270 add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
271 add_vwstack("start", "window", -u, u, -v, v);
273 if (desiredObject == D_TOOL)
275 else
277 }
278 delete[] fMit;
279 set_displayBusy(false);
280}
281
294{
295
296 if (!sceneInitialized)
297 throw;
298
299 double u;
300 double v;
301 // if(px_int != 1 && py_int != 1)
302 // we assume px_int and py_int > 0
303 if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
304 (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
305 u = (double)I.getWidth() / (2 * px_int);
306 v = (double)I.getHeight() / (2 * py_int);
307 } else {
308 u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
309 v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
310 }
311
312 float o44c[4][4], o44cd[4][4], x, y, z;
313 Matrix id = IDENTITY_MATRIX;
314
316 get_fMi(fMit);
317 this->cMo = fMit[size_fMi - 1].inverse() * fMo;
318 this->cMo = rotz * cMo;
319
320 vp2jlc_matrix(cMo.inverse(), o44c);
321 vp2jlc_matrix(cdMo.inverse(), o44cd);
322
323 while (get_displayBusy())
324 vpTime::wait(2);
325
326 add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
327 x = o44c[2][0] + o44c[3][0];
328 y = o44c[2][1] + o44c[3][1];
329 z = o44c[2][2] + o44c[3][2];
330 add_vwstack("start", "vrp", x, y, z);
331 add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
332 add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
333 add_vwstack("start", "window", -u, u, -v, v);
334 if (displayObject) {
335 display_scene(id, this->scene, I_, curColor);
336 }
337
338 add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
339 x = o44cd[2][0] + o44cd[3][0];
340 y = o44cd[2][1] + o44cd[3][1];
341 z = o44cd[2][2] + o44cd[3][2];
342 add_vwstack("start", "vrp", x, y, z);
343 add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
344 add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
345 add_vwstack("start", "window", -u, u, -v, v);
347 if (desiredObject == D_TOOL)
349 else
351 }
352 delete[] fMit;
353 set_displayBusy(false);
354}
355
362{
363 vpHomogeneousMatrix cMoTemp;
365 get_fMi(fMit);
366 cMoTemp = fMit[size_fMi - 1].inverse() * fMo;
367 delete[] fMit;
368 return cMoTemp;
369}
370
371#elif !defined(VISP_BUILD_SHARED_LIBS)
372// Work around to avoid warning:
373// libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
374void dummy_vpRobotWireFrameSimulator(){};
375#endif
void resize(unsigned int i, bool flagNullify=true)
static const vpColor red
Definition vpColor.h:211
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
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
unsigned int getHeight() const
Definition vpImage.h:184
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static Type minimum(const Type &a, const Type &b)
Definition vpMath.h:180
void unlock()
Definition vpMutex.h:106
void lock()
Definition vpMutex.h:90
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)