Visual Servoing Platform  version 3.5.1 under development (2023-06-08)
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  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(),
60  displayBusy(false),
61  robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true), cameraParam(),
62 #if defined(VISP_HAVE_DISPLAY)
63  display(),
64 #endif
65  displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false), setVelocityCalled(false),
66  verbose_(false)
67 {
68  setSamplingTime(0.010);
69  velocity.resize(6);
70  I.resize(480, 640);
71  I = 255;
72 #if defined(VISP_HAVE_DISPLAY)
73  display.init(I, 0, 0, "The External view");
74 #endif
75 }
76 
82  : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
83  artCoord(), artVel(), velocity(),
84 #if defined(_WIN32)
85 #elif defined(VISP_HAVE_PTHREAD)
86  thread(), attr(),
87 #endif
88  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(),
89  displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
90  cameraParam(),
91 #if defined(VISP_HAVE_DISPLAY)
92  display(),
93 #endif
94  displayType(MODEL_3D), displayAllowed(do_display), constantSamplingTimeMode(false), setVelocityCalled(false),
95  verbose_(false)
96 {
97  setSamplingTime(0.010);
98  velocity.resize(6);
99  I.resize(480, 640);
100  I = 255;
101 
102 #if defined(VISP_HAVE_DISPLAY)
103  if (do_display)
104  this->display.init(I, 0, 0, "The External view");
105 #endif
106 }
107 
112 {
113 }
114 
130 {
132  if (displayCamera) {
133  free_Bound_scene(&(this->camera));
134  }
135  vpWireFrameSimulator::initScene(obj, desired_object);
136  if (displayCamera) {
137  free_Bound_scene(&(this->camera));
138  }
139  displayCamera = false;
141 }
142 
154 void vpRobotWireFrameSimulator::initScene(const char *obj, const char *desired_object)
155 {
156  if (displayCamera) {
157  free_Bound_scene(&(this->camera));
158  }
159  vpWireFrameSimulator::initScene(obj, desired_object);
160  if (displayCamera) {
161  free_Bound_scene(&(this->camera));
162  }
163  displayCamera = false;
164 }
165 
179 {
180  if (displayCamera) {
181  free_Bound_scene(&(this->camera));
182  }
184  if (displayCamera) {
185  free_Bound_scene(&(this->camera));
186  }
187  displayCamera = false;
188 }
189 
201 {
202  if (displayCamera) {
203  free_Bound_scene(&(this->camera));
204  }
206  if (displayCamera) {
207  free_Bound_scene(&(this->camera));
208  }
209  displayCamera = false;
210 }
211 
224 {
225 
226  if (!sceneInitialized)
227  throw;
228 
229  double u;
230  double v;
231  // if(px_int != 1 && py_int != 1)
232  // we assume px_int and py_int > 0
233  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
234  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
235  u = (double)I_.getWidth() / (2 * px_int);
236  v = (double)I_.getHeight() / (2 * py_int);
237  } else {
238  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
239  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
240  }
241 
242  float o44c[4][4], o44cd[4][4], x, y, z;
243  Matrix id = IDENTITY_MATRIX;
244 
246  get_fMi(fMit);
247  this->cMo = fMit[size_fMi - 1].inverse() * fMo;
248  this->cMo = rotz * cMo;
249 
250  vp2jlc_matrix(cMo.inverse(), o44c);
251  vp2jlc_matrix(cdMo.inverse(), o44cd);
252 
253  while (get_displayBusy())
254  vpTime::wait(2);
255 
256  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
257  x = o44c[2][0] + o44c[3][0];
258  y = o44c[2][1] + o44c[3][1];
259  z = o44c[2][2] + o44c[3][2];
260  add_vwstack("start", "vrp", x, y, z);
261  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
262  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
263  add_vwstack("start", "window", -u, u, -v, v);
264  if (displayObject)
265  display_scene(id, this->scene, I_, curColor);
266 
267  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
268  x = o44cd[2][0] + o44cd[3][0];
269  y = o44cd[2][1] + o44cd[3][1];
270  z = o44cd[2][2] + o44cd[3][2];
271  add_vwstack("start", "vrp", x, y, z);
272  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
273  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
274  add_vwstack("start", "window", -u, u, -v, v);
275  if (displayDesiredObject) {
276  if (desiredObject == D_TOOL)
278  else
280  }
281  delete[] fMit;
282  set_displayBusy(false);
283 }
284 
297 {
298 
299  if (!sceneInitialized)
300  throw;
301 
302  double u;
303  double v;
304  // if(px_int != 1 && py_int != 1)
305  // we assume px_int and py_int > 0
306  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
307  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
308  u = (double)I.getWidth() / (2 * px_int);
309  v = (double)I.getHeight() / (2 * py_int);
310  } else {
311  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
312  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
313  }
314 
315  float o44c[4][4], o44cd[4][4], x, y, z;
316  Matrix id = IDENTITY_MATRIX;
317 
319  get_fMi(fMit);
320  this->cMo = fMit[size_fMi - 1].inverse() * fMo;
321  this->cMo = rotz * cMo;
322 
323  vp2jlc_matrix(cMo.inverse(), o44c);
324  vp2jlc_matrix(cdMo.inverse(), o44cd);
325 
326  while (get_displayBusy())
327  vpTime::wait(2);
328 
329  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
330  x = o44c[2][0] + o44c[3][0];
331  y = o44c[2][1] + o44c[3][1];
332  z = o44c[2][2] + o44c[3][2];
333  add_vwstack("start", "vrp", x, y, z);
334  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
335  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
336  add_vwstack("start", "window", -u, u, -v, v);
337  if (displayObject) {
338  display_scene(id, this->scene, I_, curColor);
339  }
340 
341  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
342  x = o44cd[2][0] + o44cd[3][0];
343  y = o44cd[2][1] + o44cd[3][1];
344  z = o44cd[2][2] + o44cd[3][2];
345  add_vwstack("start", "vrp", x, y, z);
346  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
347  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
348  add_vwstack("start", "window", -u, u, -v, v);
349  if (displayDesiredObject) {
350  if (desiredObject == D_TOOL)
352  else
354  }
355  delete[] fMit;
356  set_displayBusy(false);
357 }
358 
365 {
366  vpHomogeneousMatrix cMoTemp;
368  get_fMi(fMit);
369  cMoTemp = fMit[size_fMi - 1].inverse() * fMo;
370  delete[] fMit;
371  return cMoTemp;
372 }
373 
374 #elif !defined(VISP_BUILD_SHARED_LIBS)
375 // Work around to avoid warning:
376 // libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
377 void dummy_vpRobotWireFrameSimulator(){};
378 #endif
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
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:247
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
unsigned int getHeight() const
Definition: vpImage.h:189
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:170
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:178
void unlock()
Definition: vpMutex.h:111
void lock()
Definition: vpMutex.h:95
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)