Visual Servoing Platform  version 3.6.1 under development (2024-06-18)
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(VISP_HAVE_THREADS)
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 
51  : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(nullptr), size_fMi(8), fMi(nullptr),
52  artCoord(), artVel(), velocity(), m_thread(), m_mutex_fMi(), m_mutex_eMc(), m_mutex_artVel(), m_mutex_artCoord(),
53  m_mutex_velocity(), m_mutex_display(), m_mutex_robotStop(), m_mutex_frame(), m_mutex_setVelocityCalled(),
54  m_mutex_scene(), displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false),
55  singularityManagement(true), cameraParam(),
56 #if defined(VISP_HAVE_DISPLAY)
57  display(),
58 #endif
59  displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false), setVelocityCalled(false),
60  verbose_(false)
61 {
62  setSamplingTime(0.010);
63  velocity.resize(6);
64  I.resize(480, 640);
65  I = 255;
66 #if defined(VISP_HAVE_DISPLAY)
67  display.init(I, 0, 0, "The External view");
68 #endif
69 }
70 
76  : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(nullptr), size_fMi(8), fMi(nullptr),
77  artCoord(), artVel(), velocity(), m_thread(), m_mutex_fMi(), m_mutex_eMc(), m_mutex_artVel(), m_mutex_artCoord(),
78  m_mutex_velocity(), m_mutex_display(), m_mutex_robotStop(), m_mutex_frame(), m_mutex_setVelocityCalled(), m_mutex_scene(),
79  displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
80  cameraParam(),
81 #if defined(VISP_HAVE_DISPLAY)
82  display(),
83 #endif
84  displayType(MODEL_3D), displayAllowed(do_display), constantSamplingTimeMode(false), setVelocityCalled(false),
85  verbose_(false)
86 {
87  setSamplingTime(0.010);
88  velocity.resize(6);
89  I.resize(480, 640);
90  I = 255;
91 
92 #if defined(VISP_HAVE_DISPLAY)
93  if (do_display)
94  this->display.init(I, 0, 0, "The External view");
95 #endif
96 }
97 
113 {
114  m_mutex_scene.lock();
115  if (displayCamera) {
116  free_Bound_scene(&(this->camera));
117  }
118  vpWireFrameSimulator::initScene(obj, desired_object);
119  if (displayCamera) {
120  free_Bound_scene(&(this->camera));
121  }
122  displayCamera = false;
123  m_mutex_scene.unlock();
124 }
125 
137 void vpRobotWireFrameSimulator::initScene(const char *obj, const char *desired_object)
138 {
139  if (displayCamera) {
140  free_Bound_scene(&(this->camera));
141  }
142  vpWireFrameSimulator::initScene(obj, desired_object);
143  if (displayCamera) {
144  free_Bound_scene(&(this->camera));
145  }
146  displayCamera = false;
147 }
148 
162 {
163  if (displayCamera) {
164  free_Bound_scene(&(this->camera));
165  }
167  if (displayCamera) {
168  free_Bound_scene(&(this->camera));
169  }
170  displayCamera = false;
171 }
172 
184 {
185  if (displayCamera) {
186  free_Bound_scene(&(this->camera));
187  }
189  if (displayCamera) {
190  free_Bound_scene(&(this->camera));
191  }
192  displayCamera = false;
193 }
194 
207 {
208 
209  if (!sceneInitialized)
210  throw;
211 
212  double u;
213  double v;
214  // if(px_int != 1 && py_int != 1)
215  // we assume px_int and py_int > 0
216  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
217  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
218  u = (double)I_.getWidth() / (2 * px_int);
219  v = (double)I_.getHeight() / (2 * py_int);
220  }
221  else {
222  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
223  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
224  }
225 
226  float o44c[4][4], o44cd[4][4], x, y, z;
227  Matrix id = IDENTITY_MATRIX;
228 
230  get_fMi(fMit);
231  this->cMo = fMit[size_fMi - 1].inverse() * fMo;
232  this->cMo = rotz * cMo;
233 
234  vp2jlc_matrix(cMo.inverse(), o44c);
235  vp2jlc_matrix(cdMo.inverse(), o44cd);
236 
237  while (get_displayBusy())
238  vpTime::wait(2);
239 
240  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
241  x = o44c[2][0] + o44c[3][0];
242  y = o44c[2][1] + o44c[3][1];
243  z = o44c[2][2] + o44c[3][2];
244  add_vwstack("start", "vrp", x, y, z);
245  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
246  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
247  add_vwstack("start", "window", -u, u, -v, v);
248  if (displayObject)
249  display_scene(id, this->scene, I_, curColor);
250 
251  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
252  x = o44cd[2][0] + o44cd[3][0];
253  y = o44cd[2][1] + o44cd[3][1];
254  z = o44cd[2][2] + o44cd[3][2];
255  add_vwstack("start", "vrp", x, y, z);
256  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
257  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
258  add_vwstack("start", "window", -u, u, -v, v);
259  if (displayDesiredObject) {
260  if (desiredObject == D_TOOL)
262  else
264  }
265  delete[] fMit;
266  set_displayBusy(false);
267 }
268 
281 {
282 
283  if (!sceneInitialized)
284  throw;
285 
286  double u;
287  double v;
288  // if(px_int != 1 && py_int != 1)
289  // we assume px_int and py_int > 0
290  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
291  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
292  u = (double)I.getWidth() / (2 * px_int);
293  v = (double)I.getHeight() / (2 * py_int);
294  }
295  else {
296  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
297  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
298  }
299 
300  float o44c[4][4], o44cd[4][4], x, y, z;
301  Matrix id = IDENTITY_MATRIX;
302 
304  get_fMi(fMit);
305  this->cMo = fMit[size_fMi - 1].inverse() * fMo;
306  this->cMo = rotz * cMo;
307 
308  vp2jlc_matrix(cMo.inverse(), o44c);
309  vp2jlc_matrix(cdMo.inverse(), o44cd);
310 
311  while (get_displayBusy())
312  vpTime::wait(2);
313 
314  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
315  x = o44c[2][0] + o44c[3][0];
316  y = o44c[2][1] + o44c[3][1];
317  z = o44c[2][2] + o44c[3][2];
318  add_vwstack("start", "vrp", x, y, z);
319  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
320  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
321  add_vwstack("start", "window", -u, u, -v, v);
322  if (displayObject) {
323  display_scene(id, this->scene, I_, curColor);
324  }
325 
326  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
327  x = o44cd[2][0] + o44cd[3][0];
328  y = o44cd[2][1] + o44cd[3][1];
329  z = o44cd[2][2] + o44cd[3][2];
330  add_vwstack("start", "vrp", x, y, z);
331  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
332  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
333  add_vwstack("start", "window", -u, u, -v, v);
334  if (displayDesiredObject) {
335  if (desiredObject == D_TOOL)
337  else
339  }
340  delete[] fMit;
341  set_displayBusy(false);
342 }
343 
350 {
351  vpHomogeneousMatrix cMoTemp;
353  get_fMi(fMit);
354  cMoTemp = fMit[size_fMi - 1].inverse() * fMo;
355  delete[] fMit;
356  return cMoTemp;
357 }
359 #elif !defined(VISP_BUILD_SHARED_LIBS)
360 // Work around to avoid warning:
361 // libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
362 void dummy_vpRobotWireFrameSimulator() { };
363 #endif
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1066
static const vpColor red
Definition: vpColor.h:213
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
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:799
unsigned int getHeight() const
Definition: vpImage.h:185
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:262
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)