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