Visual Servoing Platform  version 3.6.0 under development (2023-09-29)
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);
66  velocity.resize(6);
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);
95  velocity.resize(6);
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 
109 {
110 }
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 
151 void 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);
272  if (displayDesiredObject) {
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);
346  if (displayDesiredObject) {
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
374 void dummy_vpRobotWireFrameSimulator(){};
375 #endif
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:351
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
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)